/*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                           License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other materials provided with the distribution.
//
//   * The name of the copyright holders may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/

#include "precomp.hpp"

namespace cv {

/****************************************************************************************\
*                                         GEMM                                           *
\****************************************************************************************/

static void
GEMM_CopyBlock( const uchar* src, size_t src_step,
				uchar* dst, size_t dst_step,
				Size size, size_t pix_size ) {
	int j;
	size.width *= (int)(pix_size / sizeof(int));

	for ( ; size.height--; src += src_step, dst += dst_step ) {
		for ( j = 0; j <= size.width - 4; j += 4 ) {
			int t0 = ((const int*)src)[j];
			int t1 = ((const int*)src)[j+1];
			((int*)dst)[j] = t0;
			((int*)dst)[j+1] = t1;
			t0 = ((const int*)src)[j+2];
			t1 = ((const int*)src)[j+3];
			((int*)dst)[j+2] = t0;
			((int*)dst)[j+3] = t1;
		}

		for ( ; j < size.width; j++ ) {
			((int*)dst)[j] = ((const int*)src)[j];
		}
	}
}


static void
GEMM_TransposeBlock( const uchar* src, size_t src_step,
					 uchar* dst, size_t dst_step,
					 Size size, size_t pix_size ) {
	int i, j;
	for ( i = 0; i < size.width; i++, dst += dst_step, src += pix_size ) {
		const uchar* _src = src;
		switch ( pix_size ) {
		case sizeof(int):
			for ( j = 0; j < size.height; j++, _src += src_step ) {
				((int*)dst)[j] = ((int*)_src)[0];
			}
			break;
		case sizeof(int)*2:
			for ( j = 0; j < size.height * 2; j += 2, _src += src_step ) {
				int t0 = ((int*)_src)[0];
				int t1 = ((int*)_src)[1];
				((int*)dst)[j] = t0;
				((int*)dst)[j+1] = t1;
			}
			break;
		case sizeof(int)*4:
			for ( j = 0; j < size.height * 4; j += 4, _src += src_step ) {
				int t0 = ((int*)_src)[0];
				int t1 = ((int*)_src)[1];
				((int*)dst)[j] = t0;
				((int*)dst)[j+1] = t1;
				t0 = ((int*)_src)[2];
				t1 = ((int*)_src)[3];
				((int*)dst)[j+2] = t0;
				((int*)dst)[j+3] = t1;
			}
			break;
		default:
			assert(0);
			return;
		}
	}
}


template<typename T, typename WT> static void
GEMMSingleMul( const T* a_data, size_t a_step,
			   const T* b_data, size_t b_step,
			   const T* c_data, size_t c_step,
			   T* d_data, size_t d_step,
			   Size a_size, Size d_size,
			   double alpha, double beta, int flags ) {
	int i, j, k, n = a_size.width, m = d_size.width, drows = d_size.height;
	const T* _a_data = a_data, *_b_data = b_data, *_c_data = c_data;
	T* a_buf = 0;
	size_t a_step0, a_step1, c_step0, c_step1, t_step;

	a_step /= sizeof(a_data[0]);
	b_step /= sizeof(b_data[0]);
	c_step /= sizeof(c_data[0]);
	d_step /= sizeof(d_data[0]);
	a_step0 = a_step;
	a_step1 = 1;

	if ( !c_data ) {
		c_step0 = c_step1 = 0;
	} else if ( !(flags & GEMM_3_T) ) {
		c_step0 = c_step, c_step1 = 1;
	} else {
		c_step0 = 1, c_step1 = c_step;
	}

	if ( flags & GEMM_1_T ) {
		CV_SWAP( a_step0, a_step1, t_step );
		n = a_size.height;
		if ( a_step > 1 && n > 1 ) {
			a_buf = (T*)cvStackAlloc(n * sizeof(a_data[0]));
		}
	}

	if ( n == 1 ) { /* external product */
		T* b_buf = 0;

		if ( a_step > 1 && a_size.height > 1 ) {
			a_buf = (T*)cvStackAlloc(drows * sizeof(a_data[0]));
			for ( k = 0; k < drows; k++ ) {
				a_buf[k] = a_data[a_step*k];
			}
			a_data = a_buf;
		}

		if ( b_step > 1 ) {
			b_buf = (T*)cvStackAlloc(d_size.width * sizeof(b_buf[0]) );
			for ( j = 0; j < d_size.width; j++ ) {
				b_buf[j] = b_data[j*b_step];
			}
			b_data = b_buf;
		}

		for ( i = 0; i < drows; i++, _c_data += c_step0, d_data += d_step ) {
			WT al = WT(a_data[i]) * alpha;
			c_data = _c_data;
			for ( j = 0; j <= d_size.width - 2; j += 2, c_data += 2 * c_step1 ) {
				WT s0 = al * WT(b_data[j]);
				WT s1 = al * WT(b_data[j+1]);
				if ( !c_data ) {
					d_data[j] = T(s0);
					d_data[j+1] = T(s1);
				} else {
					d_data[j] = T(s0 + WT(c_data[0]) * beta);
					d_data[j+1] = T(s1 + WT(c_data[c_step1]) * beta);
				}
			}

			for ( ; j < d_size.width; j++, c_data += c_step1 ) {
				WT s0 = al * WT(b_data[j]);
				if ( !c_data ) {
					d_data[j] = T(s0);
				} else {
					d_data[j] = T(s0 + WT(c_data[0]) * beta);
				}
			}
		}
	} else if ( flags & GEMM_2_T ) { /* A * Bt */
		for ( i = 0; i < drows; i++, _a_data += a_step0, _c_data += c_step0, d_data += d_step ) {
			a_data = _a_data;
			b_data = _b_data;
			c_data = _c_data;

			if ( a_buf ) {
				for ( k = 0; k < n; k++ ) {
					a_buf[k] = a_data[a_step1*k];
				}
				a_data = a_buf;
			}

			for ( j = 0; j < d_size.width; j++, b_data += b_step,
					c_data += c_step1 ) {
				WT s0(0), s1(0), s2(0), s3(0);

				for ( k = 0; k <= n - 4; k += 4 ) {
					s0 += WT(a_data[k]) * WT(b_data[k]);
					s1 += WT(a_data[k+1]) * WT(b_data[k+1]);
					s2 += WT(a_data[k+2]) * WT(b_data[k+2]);
					s3 += WT(a_data[k+3]) * WT(b_data[k+3]);
				}

				for ( ; k < n; k++ ) {
					s0 += WT(a_data[k]) * WT(b_data[k]);
				}
				s0 = (s0 + s1 + s2 + s3) * alpha;

				if ( !c_data ) {
					d_data[j] = T(s0);
				} else {
					d_data[j] = T(s0 + WT(c_data[0]) * beta);
				}
			}
		}
	} else if ( d_size.width * sizeof(d_data[0]) <= 1600 ) {
		for ( i = 0; i < drows; i++, _a_data += a_step0,
				_c_data += c_step0,
				d_data += d_step ) {
			a_data = _a_data, c_data = _c_data;

			if ( a_buf ) {
				for ( k = 0; k < n; k++ ) {
					a_buf[k] = a_data[a_step1*k];
				}
				a_data = a_buf;
			}

			for ( j = 0; j <= m - 4; j += 4, c_data += 4 * c_step1 ) {
				const T* b = _b_data + j;
				WT s0(0), s1(0), s2(0), s3(0);

				for ( k = 0; k < n; k++, b += b_step ) {
					WT a(a_data[k]);
					s0 += a * WT(b[0]); s1 += a * WT(b[1]);
					s2 += a * WT(b[2]); s3 += a * WT(b[3]);
				}

				if ( !c_data ) {
					d_data[j] = T(s0 * alpha);
					d_data[j+1] = T(s1 * alpha);
					d_data[j+2] = T(s2 * alpha);
					d_data[j+3] = T(s3 * alpha);
				} else {
					s0 = s0 * alpha; s1 = s1 * alpha;
					s2 = s2 * alpha; s3 = s3 * alpha;
					d_data[j] = T(s0 + WT(c_data[0]) * beta);
					d_data[j+1] = T(s1 + WT(c_data[c_step1]) * beta);
					d_data[j+2] = T(s2 + WT(c_data[c_step1*2]) * beta);
					d_data[j+3] = T(s3 + WT(c_data[c_step1*3]) * beta);
				}
			}

			for ( ; j < m; j++, c_data += c_step1 ) {
				const T* b = _b_data + j;
				WT s0(0);

				for ( k = 0; k < n; k++, b += b_step ) {
					s0 += WT(a_data[k]) * WT(b[0]);
				}

				s0 = s0 * alpha;
				if ( !c_data ) {
					d_data[j] = T(s0);
				} else {
					d_data[j] = T(s0 + WT(c_data[0]) * beta);
				}
			}
		}
	} else {
		WT* d_buf = (WT*)cvStackAlloc(m * sizeof(d_buf[0]));

		for ( i = 0; i < drows; i++, _a_data += a_step0, _c_data += c_step0, d_data += d_step ) {
			a_data = _a_data;
			b_data = _b_data;
			c_data = _c_data;

			if ( a_buf ) {
				for ( k = 0; k < n; k++ ) {
					a_buf[k] = _a_data[a_step1*k];
				}
				a_data = a_buf;
			}

			for ( j = 0; j < m; j++ ) {
				d_buf[j] = WT(0);
			}

			for ( k = 0; k < n; k++, b_data += b_step ) {
				WT al(a_data[k]);

				for ( j = 0; j <= m - 4; j += 4 ) {
					WT t0 = d_buf[j] + WT(b_data[j]) * al;
					WT t1 = d_buf[j+1] + WT(b_data[j+1]) * al;
					d_buf[j] = t0;
					d_buf[j+1] = t1;
					t0 = d_buf[j+2] + WT(b_data[j+2]) * al;
					t1 = d_buf[j+3] + WT(b_data[j+3]) * al;
					d_buf[j+2] = t0;
					d_buf[j+3] = t1;
				}

				for ( ; j < m; j++ ) {
					d_buf[j] += WT(b_data[j]) * al;
				}
			}

			if ( !c_data )
				for ( j = 0; j < m; j++ ) {
					d_data[j] = T(d_buf[j] * alpha);
				}
			else
				for ( j = 0; j < m; j++, c_data += c_step1 ) {
					WT t = d_buf[j] * alpha;
					d_data[j] = T(t + WT(c_data[0]) * beta);
				}
		}
	}
}


template<typename T, typename WT> static void
GEMMBlockMul( const T* a_data, size_t a_step,
			  const T* b_data, size_t b_step,
			  WT* d_data, size_t d_step,
			  Size a_size, Size d_size, int flags ) {
	int i, j, k, n = a_size.width, m = d_size.width;
	const T* _a_data = a_data, *_b_data = b_data;
	T* a_buf = 0;
	size_t a_step0, a_step1, t_step;
	int do_acc = flags & 16;

	a_step /= sizeof(a_data[0]);
	b_step /= sizeof(b_data[0]);
	d_step /= sizeof(d_data[0]);

	a_step0 = a_step;
	a_step1 = 1;

	if ( flags & GEMM_1_T ) {
		CV_SWAP( a_step0, a_step1, t_step );
		n = a_size.height;
		a_buf = (T*)cvStackAlloc(n * sizeof(a_data[0]));
	}

	if ( flags & GEMM_2_T ) {
		/* second operand is transposed */
		for ( i = 0; i < d_size.height; i++, _a_data += a_step0, d_data += d_step ) {
			a_data = _a_data; b_data = _b_data;

			if ( a_buf ) {
				for ( k = 0; k < n; k++ ) {
					a_buf[k] = a_data[a_step1*k];
				}
				a_data = a_buf;
			}

			for ( j = 0; j < d_size.width; j++, b_data += b_step ) {
				WT s0 = do_acc ? d_data[j] : WT(0), s1(0);
				for ( k = 0; k <= n - 2; k += 2 ) {
					s0 += WT(a_data[k]) * WT(b_data[k]);
					s1 += WT(a_data[k+1]) * WT(b_data[k+1]);
				}

				for ( ; k < n; k++ ) {
					s0 += WT(a_data[k]) * WT(b_data[k]);
				}

				d_data[j] = s0 + s1;
			}
		}
	} else {
		for ( i = 0; i < d_size.height; i++, _a_data += a_step0, d_data += d_step ) {
			a_data = _a_data, b_data = _b_data;

			if ( a_buf ) {
				for ( k = 0; k < n; k++ ) {
					a_buf[k] = a_data[a_step1*k];
				}
				a_data = a_buf;
			}

			for ( j = 0; j <= m - 4; j += 4 ) {
				WT s0, s1, s2, s3;
				const T* b = b_data + j;

				if ( do_acc ) {
					s0 = d_data[j]; s1 = d_data[j+1];
					s2 = d_data[j+2]; s3 = d_data[j+3];
				} else {
					s0 = s1 = s2 = s3 = WT(0);
				}

				for ( k = 0; k < n; k++, b += b_step ) {
					WT a(a_data[k]);
					s0 += a * WT(b[0]); s1 += a * WT(b[1]);
					s2 += a * WT(b[2]); s3 += a * WT(b[3]);
				}

				d_data[j] = s0; d_data[j+1] = s1;
				d_data[j+2] = s2; d_data[j+3] = s3;
			}

			for ( ; j < m; j++ ) {
				const T* b = b_data + j;
				WT s0 = do_acc ? d_data[j] : WT(0);

				for ( k = 0; k < n; k++, b += b_step ) {
					s0 += WT(a_data[k]) * WT(b[0]);
				}

				d_data[j] = s0;
			}
		}
	}
}


template<typename T, typename WT> static void
GEMMStore( const T* c_data, size_t c_step,
		   const WT* d_buf, size_t d_buf_step,
		   T* d_data, size_t d_step, Size d_size,
		   double alpha, double beta, int flags ) {
	const T* _c_data = c_data;
	int j;
	size_t c_step0, c_step1;

	c_step /= sizeof(c_data[0]);
	d_buf_step /= sizeof(d_buf[0]);
	d_step /= sizeof(d_data[0]);

	if ( !c_data ) {
		c_step0 = c_step1 = 0;
	} else if ( !(flags & GEMM_3_T) ) {
		c_step0 = c_step, c_step1 = 1;
	} else {
		c_step0 = 1, c_step1 = c_step;
	}

	for ( ; d_size.height--; _c_data += c_step0, d_buf += d_buf_step, d_data += d_step ) {
		if ( _c_data ) {
			c_data = _c_data;
			for ( j = 0; j <= d_size.width - 4; j += 4, c_data += 4 * c_step1 ) {
				WT t0 = alpha * d_buf[j];
				WT t1 = alpha * d_buf[j+1];
				t0 += beta * WT(c_data[0]);
				t1 += beta * WT(c_data[c_step1]);
				d_data[j] = T(t0);
				d_data[j+1] = T(t1);
				t0 = alpha * d_buf[j+2];
				t1 = alpha * d_buf[j+3];
				t0 += beta * WT(c_data[c_step1*2]);
				t1 += beta * WT(c_data[c_step1*3]);
				d_data[j+2] = T(t0);
				d_data[j+3] = T(t1);
			}
			for ( ; j < d_size.width; j++, c_data += c_step1 ) {
				WT t0 = alpha * d_buf[j];
				d_data[j] = T(t0 + WT(c_data[0]) * beta);
			}
		} else {
			for ( j = 0; j <= d_size.width - 4; j += 4 ) {
				WT t0 = alpha * d_buf[j];
				WT t1 = alpha * d_buf[j+1];
				d_data[j] = T(t0);
				d_data[j+1] = T(t1);
				t0 = alpha * d_buf[j+2];
				t1 = alpha * d_buf[j+3];
				d_data[j+2] = T(t0);
				d_data[j+3] = T(t1);
			}
			for ( ; j < d_size.width; j++ ) {
				d_data[j] = T(alpha * d_buf[j]);
			}
		}
	}
}


typedef void (*GEMMSingleMulFunc)( const void* src1, size_t step1,
								   const void* src2, size_t step2, const void* src3, size_t step3,
								   void* dst, size_t dststep, Size srcsize, Size dstsize,
								   double alpha, double beta, int flags );

typedef void (*GEMMBlockMulFunc)( const void* src1, size_t step1,
								  const void* src2, size_t step2, void* dst, size_t dststep,
								  Size srcsize, Size dstsize, int flags );

typedef void (*GEMMStoreFunc)( const void* src1, size_t step1,
							   const void* src2, size_t step2, void* dst, size_t dststep,
							   Size dstsize, double alpha, double beta, int flags );

static void GEMMSingleMul_32f( const float* a_data, size_t a_step,
							   const float* b_data, size_t b_step,
							   const float* c_data, size_t c_step,
							   float* d_data, size_t d_step,
							   Size a_size, Size d_size,
							   double alpha, double beta, int flags ) {
	GEMMSingleMul<float, double>(a_data, a_step, b_data, b_step, c_data,
								 c_step, d_data, d_step, a_size, d_size,
								 alpha, beta, flags);
}

static void GEMMSingleMul_64f( const double* a_data, size_t a_step,
							   const double* b_data, size_t b_step,
							   const double* c_data, size_t c_step,
							   double* d_data, size_t d_step,
							   Size a_size, Size d_size,
							   double alpha, double beta, int flags ) {
	GEMMSingleMul<double, double>(a_data, a_step, b_data, b_step, c_data,
								  c_step, d_data, d_step, a_size, d_size,
								  alpha, beta, flags);
}


static void GEMMSingleMul_32fc( const Complexf* a_data, size_t a_step,
								const Complexf* b_data, size_t b_step,
								const Complexf* c_data, size_t c_step,
								Complexf* d_data, size_t d_step,
								Size a_size, Size d_size,
								double alpha, double beta, int flags ) {
	GEMMSingleMul<Complexf, Complexd>(a_data, a_step, b_data, b_step, c_data,
									  c_step, d_data, d_step, a_size, d_size,
									  alpha, beta, flags);
}

static void GEMMSingleMul_64fc( const Complexd* a_data, size_t a_step,
								const Complexd* b_data, size_t b_step,
								const Complexd* c_data, size_t c_step,
								Complexd* d_data, size_t d_step,
								Size a_size, Size d_size,
								double alpha, double beta, int flags ) {
	GEMMSingleMul<Complexd, Complexd>(a_data, a_step, b_data, b_step, c_data,
									  c_step, d_data, d_step, a_size, d_size,
									  alpha, beta, flags);
}

static void GEMMBlockMul_32f( const float* a_data, size_t a_step,
							  const float* b_data, size_t b_step,
							  double* d_data, size_t d_step,
							  Size a_size, Size d_size, int flags ) {
	GEMMBlockMul(a_data, a_step, b_data, b_step, d_data, d_step, a_size, d_size, flags);
}


static void GEMMBlockMul_64f( const double* a_data, size_t a_step,
							  const double* b_data, size_t b_step,
							  double* d_data, size_t d_step,
							  Size a_size, Size d_size, int flags ) {
	GEMMBlockMul(a_data, a_step, b_data, b_step, d_data, d_step, a_size, d_size, flags);
}


static void GEMMBlockMul_32fc( const Complexf* a_data, size_t a_step,
							   const Complexf* b_data, size_t b_step,
							   Complexd* d_data, size_t d_step,
							   Size a_size, Size d_size, int flags ) {
	GEMMBlockMul(a_data, a_step, b_data, b_step, d_data, d_step, a_size, d_size, flags);
}


static void GEMMBlockMul_64fc( const Complexd* a_data, size_t a_step,
							   const Complexd* b_data, size_t b_step,
							   Complexd* d_data, size_t d_step,
							   Size a_size, Size d_size, int flags ) {
	GEMMBlockMul(a_data, a_step, b_data, b_step, d_data, d_step, a_size, d_size, flags);
}


static void GEMMStore_32f( const float* c_data, size_t c_step,
						   const double* d_buf, size_t d_buf_step,
						   float* d_data, size_t d_step, Size d_size,
						   double alpha, double beta, int flags ) {
	GEMMStore(c_data, c_step, d_buf, d_buf_step, d_data, d_step, d_size, alpha, beta, flags);
}


static void GEMMStore_64f( const double* c_data, size_t c_step,
						   const double* d_buf, size_t d_buf_step,
						   double* d_data, size_t d_step, Size d_size,
						   double alpha, double beta, int flags ) {
	GEMMStore(c_data, c_step, d_buf, d_buf_step, d_data, d_step, d_size, alpha, beta, flags);
}


static void GEMMStore_32fc( const Complexf* c_data, size_t c_step,
							const Complexd* d_buf, size_t d_buf_step,
							Complexf* d_data, size_t d_step, Size d_size,
							double alpha, double beta, int flags ) {
	GEMMStore(c_data, c_step, d_buf, d_buf_step, d_data, d_step, d_size, alpha, beta, flags);
}


static void GEMMStore_64fc( const Complexd* c_data, size_t c_step,
							const Complexd* d_buf, size_t d_buf_step,
							Complexd* d_data, size_t d_step, Size d_size,
							double alpha, double beta, int flags ) {
	GEMMStore(c_data, c_step, d_buf, d_buf_step, d_data, d_step, d_size, alpha, beta, flags);
}


void gemm( const Mat& matA, const Mat& matB, double alpha,
		   const Mat& matC, double beta, Mat& D, int flags ) {
	const int block_lin_size = 128;
	const int block_size = block_lin_size * block_lin_size;

	static double zero[] = {0, 0, 0, 0};
	static float zerof[] = {0, 0, 0, 0};

	Mat A = matA, B = matB;
	const Mat* C = matC.data && beta != 0 ? &matC : 0;
	Size a_size = A.size(), d_size;
	int i, len = 0, type = A.type();

	CV_Assert( type == B.type() && (type == CV_32FC1 || type == CV_64FC1 || type == CV_32FC2 || type == CV_64FC2) );

	switch ( flags & (GEMM_1_T | GEMM_2_T) ) {
	case 0:
		d_size = Size( B.cols, a_size.height );
		len = B.rows;
		CV_Assert( a_size.width == len );
		break;
	case 1:
		d_size = Size( B.cols, a_size.width );
		len = B.rows;
		CV_Assert( a_size.height == len );
		break;
	case 2:
		d_size = Size( B.rows, a_size.height );
		len = B.cols;
		CV_Assert( a_size.width == len );
		break;
	case 3:
		d_size = Size( B.rows, a_size.width );
		len = B.cols;
		CV_Assert( a_size.height == len );
		break;
	}

	if ( C ) {
		CV_Assert( C->type() == type &&
				   (((flags & GEMM_3_T) == 0 && C->rows == d_size.height && C->cols == d_size.width) ||
					((flags & GEMM_3_T) != 0 && C->rows == d_size.width && C->cols == d_size.height)));
		if ( (flags & GEMM_3_T) != 0 && C->data == D.data ) {
			transpose( D, D );
			flags &= ~GEMM_3_T;
		}
	}

	D.create( d_size.height, d_size.width, type );

	if ( flags == 0 && 2 <= len && len <= 4 && (len == d_size.width || len == d_size.height) ) {
		if ( type == CV_32F ) {
			float* d = (float*)D.data;
			const float* a = (const float*)A.data,
						 *b = (const float*)B.data,
						  *c = (const float*)(C ? C->data : 0);
			size_t d_step = D.step / sizeof(d[0]),
				   a_step = A.step / sizeof(a[0]),
				   b_step = B.step / sizeof(b[0]),
				   c_step = C ? C->step / sizeof(c[0]) : 0;

			if ( !c ) {
				c = zerof;
			}

			switch ( len ) {
			case 2:
				if ( len == d_size.width && b != d ) {
					for ( i = 0; i < d_size.height; i++, d += d_step, a += a_step, c += c_step ) {
						float t0 = a[0] * b[0] + a[1] * b[b_step];
						float t1 = a[0] * b[1] + a[1] * b[b_step+1];
						d[0] = (float)(t0 * alpha + c[0] * beta);
						d[1] = (float)(t1 * alpha + c[1] * beta);
					}
				} else if ( a != d ) {
					int c_step0 = 1;
					if ( c == zerof ) {
						c_step0 = 0;
						c_step = 1;
					}

					for ( i = 0; i < d_size.width; i++, d++, b++, c += c_step0 ) {
						float t0 = a[0] * b[0] + a[1] * b[b_step];
						float t1 = a[a_step] * b[0] + a[a_step+1] * b[b_step];
						d[0] = (float)(t0 * alpha + c[0] * beta);
						d[d_step] = (float)(t1 * alpha + c[c_step] * beta);
					}
				} else {
					break;
				}
				return;
			case 3:
				if ( len == d_size.width && b != d ) {
					for ( i = 0; i < d_size.height; i++, d += d_step, a += a_step, c += c_step ) {
						float t0 = a[0] * b[0] + a[1] * b[b_step] + a[2] * b[b_step*2];
						float t1 = a[0] * b[1] + a[1] * b[b_step+1] + a[2] * b[b_step*2+1];
						float t2 = a[0] * b[2] + a[1] * b[b_step+2] + a[2] * b[b_step*2+2];
						d[0] = (float)(t0 * alpha + c[0] * beta);
						d[1] = (float)(t1 * alpha + c[1] * beta);
						d[2] = (float)(t2 * alpha + c[2] * beta);
					}
				} else if ( a != d ) {
					int c_step0 = 1;
					if ( c == zerof ) {
						c_step0 = 0;
						c_step = 1;
					}

					for ( i = 0; i < d_size.width; i++, d++, b++, c += c_step0 ) {
						float t0 = a[0] * b[0] + a[1] * b[b_step] + a[2] * b[b_step*2];
						float t1 = a[a_step] * b[0] + a[a_step+1] * b[b_step] + a[a_step+2] * b[b_step*2];
						float t2 = a[a_step*2] * b[0] + a[a_step*2+1] * b[b_step] + a[a_step*2+2] * b[b_step*2];

						d[0] = (float)(t0 * alpha + c[0] * beta);
						d[d_step] = (float)(t1 * alpha + c[c_step] * beta);
						d[d_step*2] = (float)(t2 * alpha + c[c_step*2] * beta);
					}
				} else {
					break;
				}
				return;
			case 4:
				if ( len == d_size.width && b != d ) {
					for ( i = 0; i < d_size.height; i++, d += d_step, a += a_step, c += c_step ) {
						float t0 = a[0] * b[0] + a[1] * b[b_step] + a[2] * b[b_step*2] + a[3] * b[b_step*3];
						float t1 = a[0] * b[1] + a[1] * b[b_step+1] + a[2] * b[b_step*2+1] + a[3] * b[b_step*3+1];
						float t2 = a[0] * b[2] + a[1] * b[b_step+2] + a[2] * b[b_step*2+2] + a[3] * b[b_step*3+2];
						float t3 = a[0] * b[3] + a[1] * b[b_step+3] + a[2] * b[b_step*2+3] + a[3] * b[b_step*3+3];
						d[0] = (float)(t0 * alpha + c[0] * beta);
						d[1] = (float)(t1 * alpha + c[1] * beta);
						d[2] = (float)(t2 * alpha + c[2] * beta);
						d[3] = (float)(t3 * alpha + c[3] * beta);
					}
				} else if ( len <= 16 && a != d ) {
					int c_step0 = 1;
					if ( c == zerof ) {
						c_step0 = 0;
						c_step = 1;
					}

					for ( i = 0; i < d_size.width; i++, d++, b++, c += c_step0 ) {
						float t0 = a[0] * b[0] + a[1] * b[b_step] + a[2] * b[b_step*2] + a[3] * b[b_step*3];
						float t1 = a[a_step] * b[0] + a[a_step+1] * b[b_step] +
								   a[a_step+2] * b[b_step*2] + a[a_step+3] * b[b_step*3];
						float t2 = a[a_step*2] * b[0] + a[a_step*2+1] * b[b_step] +
								   a[a_step*2+2] * b[b_step*2] + a[a_step*2+3] * b[b_step*3];
						float t3 = a[a_step*3] * b[0] + a[a_step*3+1] * b[b_step] +
								   a[a_step*3+2] * b[b_step*2] + a[a_step*3+3] * b[b_step*3];
						d[0] = (float)(t0 * alpha + c[0] * beta);
						d[d_step] = (float)(t1 * alpha + c[c_step] * beta);
						d[d_step*2] = (float)(t2 * alpha + c[c_step*2] * beta);
						d[d_step*3] = (float)(t3 * alpha + c[c_step*3] * beta);
					}
				} else {
					break;
				}
				return;
			}
		}

		if ( type == CV_64F ) {
			double* d = (double*)D.data;
			const double* a = (const double*)A.data,
						  *b = (const double*)B.data,
						   *c = (const double*)(C ? C->data : 0);
			size_t d_step = D.step / sizeof(d[0]),
				   a_step = A.step / sizeof(a[0]),
				   b_step = B.step / sizeof(b[0]),
				   c_step = C ? C->step / sizeof(c[0]) : 0;
			if ( !c ) {
				c = zero;
			}

			switch ( len ) {
			case 2:
				if ( len == d_size.width && b != d ) {
					for ( i = 0; i < d_size.height; i++, d += d_step, a += a_step, c += c_step ) {
						double t0 = a[0] * b[0] + a[1] * b[b_step];
						double t1 = a[0] * b[1] + a[1] * b[b_step+1];
						d[0] = t0 * alpha + c[0] * beta;
						d[1] = t1 * alpha + c[1] * beta;
					}
				} else if ( a != d ) {
					int c_step0 = 1;
					if ( c == zero ) {
						c_step0 = 0;
						c_step = 1;
					}

					for ( i = 0; i < d_size.width; i++, d++, b++, c += c_step0 ) {
						double t0 = a[0] * b[0] + a[1] * b[b_step];
						double t1 = a[a_step] * b[0] + a[a_step+1] * b[b_step];
						d[0] = t0 * alpha + c[0] * beta;
						d[d_step] = t1 * alpha + c[c_step] * beta;
					}
				} else {
					break;
				}
				return;
			case 3:
				if ( len == d_size.width && b != d ) {
					for ( i = 0; i < d_size.height; i++, d += d_step, a += a_step, c += c_step ) {
						double t0 = a[0] * b[0] + a[1] * b[b_step] + a[2] * b[b_step*2];
						double t1 = a[0] * b[1] + a[1] * b[b_step+1] + a[2] * b[b_step*2+1];
						double t2 = a[0] * b[2] + a[1] * b[b_step+2] + a[2] * b[b_step*2+2];
						d[0] = t0 * alpha + c[0] * beta;
						d[1] = t1 * alpha + c[1] * beta;
						d[2] = t2 * alpha + c[2] * beta;
					}
				} else if ( a != d ) {
					int c_step0 = 1;
					if ( c == zero ) {
						c_step0 = 0;
						c_step = 1;
					}

					for ( i = 0; i < d_size.width; i++, d++, b++, c += c_step0 ) {
						double t0 = a[0] * b[0] + a[1] * b[b_step] + a[2] * b[b_step*2];
						double t1 = a[a_step] * b[0] + a[a_step+1] * b[b_step] + a[a_step+2] * b[b_step*2];
						double t2 = a[a_step*2] * b[0] + a[a_step*2+1] * b[b_step] + a[a_step*2+2] * b[b_step*2];

						d[0] = t0 * alpha + c[0] * beta;
						d[d_step] = t1 * alpha + c[c_step] * beta;
						d[d_step*2] = t2 * alpha + c[c_step*2] * beta;
					}
				} else {
					break;
				}
				return;
			case 4:
				if ( len == d_size.width && b != d ) {
					for ( i = 0; i < d_size.height; i++, d += d_step, a += a_step, c += c_step ) {
						double t0 = a[0] * b[0] + a[1] * b[b_step] + a[2] * b[b_step*2] + a[3] * b[b_step*3];
						double t1 = a[0] * b[1] + a[1] * b[b_step+1] + a[2] * b[b_step*2+1] + a[3] * b[b_step*3+1];
						double t2 = a[0] * b[2] + a[1] * b[b_step+2] + a[2] * b[b_step*2+2] + a[3] * b[b_step*3+2];
						double t3 = a[0] * b[3] + a[1] * b[b_step+3] + a[2] * b[b_step*2+3] + a[3] * b[b_step*3+3];
						d[0] = t0 * alpha + c[0] * beta;
						d[1] = t1 * alpha + c[1] * beta;
						d[2] = t2 * alpha + c[2] * beta;
						d[3] = t3 * alpha + c[3] * beta;
					}
				} else if ( d_size.width <= 16 && a != d ) {
					int c_step0 = 1;
					if ( c == zero ) {
						c_step0 = 0;
						c_step = 1;
					}

					for ( i = 0; i < d_size.width; i++, d++, b++, c += c_step0 ) {
						double t0 = a[0] * b[0] + a[1] * b[b_step] + a[2] * b[b_step*2] + a[3] * b[b_step*3];
						double t1 = a[a_step] * b[0] + a[a_step+1] * b[b_step] +
									a[a_step+2] * b[b_step*2] + a[a_step+3] * b[b_step*3];
						double t2 = a[a_step*2] * b[0] + a[a_step*2+1] * b[b_step] +
									a[a_step*2+2] * b[b_step*2] + a[a_step*2+3] * b[b_step*3];
						double t3 = a[a_step*3] * b[0] + a[a_step*3+1] * b[b_step] +
									a[a_step*3+2] * b[b_step*2] + a[a_step*3+3] * b[b_step*3];
						d[0] = t0 * alpha + c[0] * beta;
						d[d_step] = t1 * alpha + c[c_step] * beta;
						d[d_step*2] = t2 * alpha + c[c_step*2] * beta;
						d[d_step*3] = t3 * alpha + c[c_step*3] * beta;
					}
				} else {
					break;
				}
				return;
			}
		}
	}

	{
		size_t b_step = B.step;
		GEMMSingleMulFunc singleMulFunc;
		GEMMBlockMulFunc blockMulFunc;
		GEMMStoreFunc storeFunc;
		Mat* matD = &D, tmat;
		const uchar* Cdata = C ? C->data : 0;
		size_t Cstep = C ? C->step : 0;
		AutoBuffer<uchar> buf;

		if ( type == CV_32FC1 ) {
			singleMulFunc = (GEMMSingleMulFunc)GEMMSingleMul_32f;
			blockMulFunc = (GEMMBlockMulFunc)GEMMBlockMul_32f;
			storeFunc = (GEMMStoreFunc)GEMMStore_32f;
		} else if ( type == CV_64FC1 ) {
			singleMulFunc = (GEMMSingleMulFunc)GEMMSingleMul_64f;
			blockMulFunc = (GEMMBlockMulFunc)GEMMBlockMul_64f;
			storeFunc = (GEMMStoreFunc)GEMMStore_64f;
		} else if ( type == CV_32FC2 ) {
			singleMulFunc = (GEMMSingleMulFunc)GEMMSingleMul_32fc;
			blockMulFunc = (GEMMBlockMulFunc)GEMMBlockMul_32fc;
			storeFunc = (GEMMStoreFunc)GEMMStore_32fc;
		} else {
			CV_Assert( type == CV_64FC2 );
			singleMulFunc = (GEMMSingleMulFunc)GEMMSingleMul_64fc;
			blockMulFunc = (GEMMBlockMulFunc)GEMMBlockMul_64fc;
			storeFunc = (GEMMStoreFunc)GEMMStore_64fc;
		}

		if ( D.data == A.data || D.data == B.data ) {
			buf.allocate(d_size.width * d_size.height * CV_ELEM_SIZE(type));
			tmat = Mat(d_size.height, d_size.width, type, (uchar*)buf );
			matD = &tmat;
		}

		if ( (d_size.width == 1 || len == 1) && !(flags & GEMM_2_T) && B.isContinuous() ) {
			b_step = d_size.width == 1 ? 0 : CV_ELEM_SIZE(type);
			flags |= GEMM_2_T;
		}

		/*if( (d_size.width | d_size.height | len) >= 16 && icvBLAS_GEMM_32f_p != 0 )
		{
		    blas_func = type == CV_32FC1 ? (icvBLAS_GEMM_32f_t)icvBLAS_GEMM_32f_p :
		                type == CV_64FC1 ? (icvBLAS_GEMM_32f_t)icvBLAS_GEMM_64f_p :
		                type == CV_32FC2 ? (icvBLAS_GEMM_32f_t)icvBLAS_GEMM_32fc_p :
		                type == CV_64FC2 ? (icvBLAS_GEMM_32f_t)icvBLAS_GEMM_64fc_p : 0;
		}

		if( blas_func )
		{
		    const char* transa = flags & GEMM_1_T ? "t" : "n";
		    const char* transb = flags & GEMM_2_T ? "t" : "n";
		    int lda, ldb, ldd;

		    if( C->data.ptr )
		    {
		        if( C->data.ptr != D->data.ptr )
		        {
		            if( !(flags & GEMM_3_T) )
		                cvCopy( C, D );
		            else
		                cvTranspose( C, D );
		        }
		    }

		    if( CV_MAT_DEPTH(type) == CV_32F )
		    {
		        Complex32f _alpha, _beta;

		        lda = A->step/sizeof(float);
		        ldb = b_step/sizeof(float);
		        ldd = D->step/sizeof(float);
		        _alpha.re = (float)alpha;
		        _alpha.im = 0;
		        _beta.re = C->data.ptr ? (float)beta : 0;
		        _beta.im = 0;
		        if( CV_MAT_CN(type) == 2 )
		            lda /= 2, ldb /= 2, ldd /= 2;

		        blas_func( transb, transa, &d_size.width, &d_size.height, &len,
		               &_alpha, B->data.ptr, &ldb, A->data.ptr, &lda,
		               &_beta, D->data.ptr, &ldd );
		    }
		    else
		    {
		        CvComplex64f _alpha, _beta;

		        lda = A->step/sizeof(double);
		        ldb = b_step/sizeof(double);
		        ldd = D->step/sizeof(double);
		        _alpha.re = alpha;
		        _alpha.im = 0;
		        _beta.re = C->data.ptr ? beta : 0;
		        _beta.im = 0;
		        if( CV_MAT_CN(type) == 2 )
		            lda /= 2, ldb /= 2, ldd /= 2;

		        blas_func( transb, transa, &d_size.width, &d_size.height, &len,
		               &_alpha, B->data.ptr, &ldb, A->data.ptr, &lda,
		               &_beta, D->data.ptr, &ldd );
		    }
		}
		else*/ if ( ((d_size.height <= block_lin_size / 2 || d_size.width <= block_lin_size / 2) &&
					 len <= 10000) || len <= 10 ||
					(d_size.width <= block_lin_size &&
					 d_size.height <= block_lin_size && len <= block_lin_size) ) {
			singleMulFunc( A.data, A.step, B.data, b_step, Cdata, Cstep,
						   matD->data, matD->step, a_size, d_size, alpha, beta, flags );
		} else {
			int is_a_t = flags & GEMM_1_T;
			int is_b_t = flags & GEMM_2_T;
			int elem_size = CV_ELEM_SIZE(type);
			int dk0_1, dk0_2;
			int a_buf_size = 0, b_buf_size, d_buf_size;
			uchar* a_buf = 0;
			uchar* b_buf = 0;
			uchar* d_buf = 0;
			int j, k, di = 0, dj = 0, dk = 0;
			int dm0, dn0, dk0;
			size_t a_step0, a_step1, b_step0, b_step1, c_step0, c_step1;
			int work_elem_size = elem_size << (CV_MAT_DEPTH(type) == CV_32F ? 1 : 0);

			if ( !is_a_t ) {
				a_step0 = A.step, a_step1 = elem_size;
			} else {
				a_step0 = elem_size, a_step1 = A.step;
			}

			if ( !is_b_t ) {
				b_step0 = b_step, b_step1 = elem_size;
			} else {
				b_step0 = elem_size, b_step1 = b_step;
			}

			if ( !C ) {
				c_step0 = c_step1 = 0;
				flags &= ~GEMM_3_T;
			} else if ( !(flags & GEMM_3_T) ) {
				c_step0 = C->step, c_step1 = elem_size;
			} else {
				c_step0 = elem_size, c_step1 = C->step;
			}

			dm0 = std::min( block_lin_size, d_size.height );
			dn0 = std::min( block_lin_size, d_size.width );
			dk0_1 = block_size / dm0;
			dk0_2 = block_size / dn0;
			dk0 = std::min( dk0_1, dk0_2 );
			dk0 = std::min( dk0, len );
			if ( dk0 * dm0 > block_size ) {
				dm0 = block_size / dk0;
			}
			if ( dk0 * dn0 > block_size ) {
				dn0 = block_size / dk0;
			}

			dk0_1 = (dn0 + dn0 / 8 + 2) & -2;
			b_buf_size = (dk0 + dk0 / 8 + 1) * dk0_1 * elem_size;
			d_buf_size = (dk0 + dk0 / 8 + 1) * dk0_1 * work_elem_size;

			if ( is_a_t ) {
				a_buf_size = (dm0 + dm0 / 8 + 1) * ((dk0 + dk0 / 8 + 2) & -2) * elem_size;
				flags &= ~GEMM_1_T;
			}

			buf.allocate(a_buf_size + b_buf_size + d_buf_size);
			d_buf = (uchar*)buf;
			b_buf = d_buf + d_buf_size;

			if ( is_a_t ) {
				a_buf = b_buf + b_buf_size;
			}

			for ( i = 0; i < d_size.height; i += di ) {
				di = dm0;
				if ( i + di >= d_size.height || 8 * (i + di) + di > 8 * d_size.height ) {
					di = d_size.height - i;
				}

				for ( j = 0; j < d_size.width; j += dj ) {
					uchar* _d = matD->data + i * matD->step + j * elem_size;
					const uchar* _c = Cdata + i * c_step0 + j * c_step1;
					size_t _d_step = matD->step;
					dj = dn0;

					if ( j + dj >= d_size.width || 8 * (j + dj) + dj > 8 * d_size.width ) {
						dj = d_size.width - j;
					}

					flags &= 15;
					if ( dk0 < len ) {
						_d = d_buf;
						_d_step = dj * work_elem_size;
					}

					for ( k = 0; k < len; k += dk ) {
						const uchar* _a = A.data + i * a_step0 + k * a_step1;
						size_t _a_step = A.step;
						const uchar* _b = B.data + k * b_step0 + j * b_step1;
						size_t _b_step = b_step;
						Size a_bl_size;

						dk = dk0;
						if ( k + dk >= len || 8 * (k + dk) + dk > 8 * len ) {
							dk = len - k;
						}

						if ( !is_a_t ) {
							a_bl_size.width = dk, a_bl_size.height = di;
						} else {
							a_bl_size.width = di, a_bl_size.height = dk;
						}

						if ( a_buf && is_a_t ) {
							_a_step = dk * elem_size;
							GEMM_TransposeBlock( _a, A.step, a_buf, _a_step, a_bl_size, elem_size );
							std::swap( a_bl_size.width, a_bl_size.height );
							_a = a_buf;
						}

						if ( dj < d_size.width ) {
							Size b_size;
							if ( !is_b_t ) {
								b_size.width = dj, b_size.height = dk;
							} else {
								b_size.width = dk, b_size.height = dj;
							}

							_b_step = b_size.width * elem_size;
							GEMM_CopyBlock( _b, b_step, b_buf, _b_step, b_size, elem_size );
							_b = b_buf;
						}

						if ( dk0 < len )
							blockMulFunc( _a, _a_step, _b, _b_step, _d, _d_step,
										  a_bl_size, Size(dj, di), flags );
						else
							singleMulFunc( _a, _a_step, _b, _b_step, _c, Cstep,
										   _d, _d_step, a_bl_size, Size(dj, di), alpha, beta, flags );
						flags |= 16;
					}

					if ( dk0 < len )
						storeFunc( _c, Cstep, _d, _d_step,
								   matD->data + i * matD->step + j * elem_size,
								   matD->step, Size(dj, di), alpha, beta, flags );
				}
			}
		}

		if ( matD != &D ) {
			matD->copyTo(D);
		}
	}
}

/****************************************************************************************\
*                                        Transform                                       *
\****************************************************************************************/

template<typename T, typename WT> static void
transformC1_( const Mat& srcmat, Mat& dstmat, Mat& tmat ) {
	Size size = getContinuousSize( srcmat, dstmat );
	const WT* m = (const WT*)tmat.data;
	int dst_cn = dstmat.channels();
	int x, y, k;

	for ( y = 0; y < size.height; y++ ) {
		const T* src = (const T*)(srcmat.data + srcmat.step * y);
		T* dst = (T*)(dstmat.data + dstmat.step * y);
		const WT* _m = m;

		for ( k = 0; k < dst_cn; k++, dst++, _m += 2 )
			for ( x = 0; x < size.width; x++ ) {
				dst[x* dst_cn] = saturate_cast<T>(_m[0] * src[x] + _m[1]);
			}
	}
}

template<typename T, typename WT> static void
transformC2_( const Mat& srcmat, Mat& dstmat, Mat& tmat ) {
	Size size = getContinuousSize( srcmat, dstmat );
	const WT* m = (const WT*)tmat.data;
	int dst_cn = dstmat.channels();
	int x, y, k;

	for ( y = 0; y < size.height; y++ ) {
		const T* src = (const T*)(srcmat.data + srcmat.step * y);
		T* dst = (T*)(dstmat.data + dstmat.step * y);

		if ( dst_cn == 2 )
			for ( x = 0; x < size.width * 2; x += 2 ) {
				WT v0 = src[x], v1 = src[x+1];
				T t0 = saturate_cast<T>(m[0] * v0 + m[1] * v1 + m[2]);
				T t1 = saturate_cast<T>(m[3] * v0 + m[4] * v1 + m[5]);
				dst[x] = t0; dst[x+1] = t1;
			}
		else {
			const WT* _m = m;
			for ( k = 0; k < dst_cn; k++, dst++, _m += 3 )
				for ( x = 0; x < size.width; x++ )
					dst[x* dst_cn] = saturate_cast<T>(_m[0] * src[x*2] +
													  _m[1] * src[x*2+1] + _m[2]);
		}
	}
}

template<typename T, typename WT> static void
transformC3_( const Mat& srcmat, Mat& dstmat, Mat& tmat ) {
	Size size = getContinuousSize( srcmat, dstmat );
	const WT* m = (const WT*)tmat.data;
	int dst_cn = dstmat.channels();
	int x, y, k;

	for ( y = 0; y < size.height; y++ ) {
		const T* src = (const T*)(srcmat.data + srcmat.step * y);
		T* dst = (T*)(dstmat.data + dstmat.step * y);

		if ( dst_cn == 3 )
			for ( x = 0; x < size.width * 3; x += 3 ) {
				WT v0 = src[x], v1 = src[x+1], v2 = src[x+2];
				T t0 = saturate_cast<T>(m[0] * v0 + m[1] * v1 + m[2] * v2 + m[3]);
				T t1 = saturate_cast<T>(m[4] * v0 + m[5] * v1 + m[6] * v2 + m[7]);
				T t2 = saturate_cast<T>(m[8] * v0 + m[9] * v1 + m[10] * v2 + m[11]);
				dst[x] = t0; dst[x+1] = t1; dst[x+2] = t2;
			}
		else if ( dst_cn == 1 )
			for ( x = 0; x < size.width; x++, src += 3 ) {
				dst[x] = saturate_cast<T>(m[0] * src[0] + m[1] * src[1] + m[2] * src[2] + m[3]);
			}
		else {
			const WT* _m = m;
			for ( k = 0; k < dst_cn; k++, dst++, _m += 4 )
				for ( x = 0; x < size.width; x++ )
					dst[x* dst_cn] = saturate_cast<T>(_m[0] * src[x*3] +
													  _m[1] * src[x*3+1] + _m[2] * src[x*3+2] + _m[3]);
		}
	}
}


template<typename T, typename WT> static void
transformC4_( const Mat& srcmat, Mat& dstmat, Mat& tmat ) {
	Size size = getContinuousSize( srcmat, dstmat );
	const WT* m = (const WT*)tmat.data;
	int dst_cn = dstmat.channels();
	int x, y, k;

	for ( y = 0; y < size.height; y++ ) {
		const T* src = (const T*)(srcmat.data + srcmat.step * y);
		T* dst = (T*)(dstmat.data + dstmat.step * y);

		if ( dst_cn == 4 )
			for ( x = 0; x < size.width * 4; x += 4 ) {
				WT v0 = src[x], v1 = src[x+1], v2 = src[x+2], v3 = src[x+3];
				T t0 = saturate_cast<T>(m[0] * v0 + m[1] * v1 + m[2] * v2 + m[3] * v3 + m[4]);
				T t1 = saturate_cast<T>(m[5] * v0 + m[6] * v1 + m[7] * v2 + m[8] * v3 + m[9]);
				dst[x] = t0; dst[x+1] = t1;
				t0 = saturate_cast<T>(m[10] * v0 + m[11] * v1 + m[12] * v2 + m[13] * v3 + m[14]);
				t1 = saturate_cast<T>(m[15] * v0 + m[16] * v1 + m[17] * v2 + m[18] * v3 + m[19]);
				dst[x+2] = t0; dst[x+3] = t1;
			}
		else {
			const WT* _m = m;
			for ( k = 0; k < dst_cn; k++, dst++, _m += 5 )
				for ( x = 0; x < size.width; x++ )
					dst[x* dst_cn] = saturate_cast<T>(_m[0] * src[x*4] + _m[1] * src[x*4+1] +
													  _m[2] * src[x*4+2] + _m[3] * src[x*4+3] + _m[4]);
		}
	}
}


#if CV_SSE2

static inline void
load3x3Matrix( const float* m, __m128& m0, __m128& m1, __m128& m2, __m128& m3 ) {
	m0 = _mm_setr_ps(m[0], m[4], m[8], 0);
	m1 = _mm_setr_ps(m[1], m[5], m[9], 0);
	m2 = _mm_setr_ps(m[2], m[6], m[10], 0);
	m3 = _mm_setr_ps(m[3], m[7], m[11], 0);
}

static inline void
load4x4Matrix( const float* m, __m128& m0, __m128& m1, __m128& m2, __m128& m3, __m128& m4 ) {
	m0 = _mm_setr_ps(m[0], m[5], m[10], m[15]);
	m1 = _mm_setr_ps(m[1], m[6], m[11], m[16]);
	m2 = _mm_setr_ps(m[2], m[7], m[12], m[17]);
	m3 = _mm_setr_ps(m[3], m[8], m[13], m[18]);
	m4 = _mm_setr_ps(m[4], m[9], m[14], m[19]);
}

template<> void
transformC3_<uchar, float>( const Mat& srcmat, Mat& dstmat, Mat& tmat ) {
	typedef uchar T;
	typedef float WT;
	const int BITS = 10, SCALE = 1 << BITS;
	const float MAX_M = (float)(1 << (15 - BITS));
	Size size = getContinuousSize( srcmat, dstmat );
	const float* m = (const float*)tmat.data;
	int dst_cn = dstmat.channels();
	int x, y, k;

	if ( checkHardwareSupport(CV_CPU_SSE2) && dst_cn == 3 &&
			std::abs(m[0]) < MAX_M && std::abs(m[1]) < MAX_M && std::abs(m[2]) < MAX_M && std::abs(m[3]) < MAX_M * 256 &&
			std::abs(m[4]) < MAX_M && std::abs(m[5]) < MAX_M && std::abs(m[6]) < MAX_M && std::abs(m[7]) < MAX_M * 256 &&
			std::abs(m[8]) < MAX_M && std::abs(m[9]) < MAX_M && std::abs(m[10]) < MAX_M && std::abs(m[11]) < MAX_M * 256 ) {
		// faster fixed-point transformation
		short m00 = saturate_cast<short>(m[0] * SCALE), m01 = saturate_cast<short>(m[1] * SCALE),
			  m02 = saturate_cast<short>(m[2] * SCALE), m10 = saturate_cast<short>(m[4] * SCALE),
			  m11 = saturate_cast<short>(m[5] * SCALE), m12 = saturate_cast<short>(m[6] * SCALE),
			  m20 = saturate_cast<short>(m[8] * SCALE), m21 = saturate_cast<short>(m[9] * SCALE),
			  m22 = saturate_cast<short>(m[10] * SCALE);
		int m03 = saturate_cast<int>((m[3] + 0.5f) * SCALE), m13 = saturate_cast<int>((m[7] + 0.5f) * SCALE ),
			m23 = saturate_cast<int>((m[11] + 0.5f) * SCALE);

		__m128i m0 = _mm_setr_epi16(0, m00, m01, m02, m00, m01, m02, 0);
		__m128i m1 = _mm_setr_epi16(0, m10, m11, m12, m10, m11, m12, 0);
		__m128i m2 = _mm_setr_epi16(0, m20, m21, m22, m20, m21, m22, 0);
		__m128i m3 = _mm_setr_epi32(m03, m13, m23, 0);

		for ( y = 0; y < size.height; y++ ) {
			const T* src = (const T*)(srcmat.data + srcmat.step * y);
			T* dst = (T*)(dstmat.data + dstmat.step * y);
			int x = 0;

			for ( ; x <= (size.width - 8) * 3; x += 8 * 3 ) {
				__m128i z = _mm_setzero_si128(), t0, t1, t2, r0, r1;
				__m128i v0 = _mm_loadl_epi64((const __m128i*)(src + x));
				__m128i v1 = _mm_loadl_epi64((const __m128i*)(src + x + 8));
				__m128i v2 = _mm_loadl_epi64((const __m128i*)(src + x + 16)), v3;
				v0 = _mm_unpacklo_epi8(v0, z); // b0 g0 r0 b1 g1 r1 b2 g2
				v1 = _mm_unpacklo_epi8(v1, z); // r2 b3 g3 r3 b4 g4 r4 b5
				v2 = _mm_unpacklo_epi8(v2, z); // g5 r5 b6 g6 r6 b7 g7 r7

				v3 = _mm_srli_si128(v2, 2); // ? b6 g6 r6 b7 g7 r7 0
				v2 = _mm_or_si128(_mm_slli_si128(v2, 10), _mm_srli_si128(v1, 6)); // ? b4 g4 r4 b5 g5 r5 ?
				v1 = _mm_or_si128(_mm_slli_si128(v1, 6), _mm_srli_si128(v0, 10)); // ? b2 g2 r2 b3 g3 r3 ?
				v0 = _mm_slli_si128(v0, 2); // 0 b0 g0 r0 b1 g1 r1 ?

				// process pixels 0 & 1
				t0 = _mm_madd_epi16(v0, m0); // a0 b0 a1 b1
				t1 = _mm_madd_epi16(v0, m1); // c0 d0 c1 d1
				t2 = _mm_madd_epi16(v0, m2); // e0 f0 e1 f1
				v0 = _mm_unpacklo_epi32(t0, t1); // a0 c0 b0 d0
				t0 = _mm_unpackhi_epi32(t0, t1); // a1 b1 c1 d1
				t1 = _mm_unpacklo_epi32(t2, z);  // e0 0 f0 0
				t2 = _mm_unpackhi_epi32(t2, z);  // e1 0 f1 0
				r0 = _mm_add_epi32(_mm_add_epi32(_mm_unpacklo_epi64(v0, t1), _mm_unpackhi_epi64(v0, t1)), m3); // B0 G0 R0 0
				r1 = _mm_add_epi32(_mm_add_epi32(_mm_unpacklo_epi64(t0, t2), _mm_unpackhi_epi64(t0, t2)), m3); // B1 G1 R1 0
				r0 = _mm_srai_epi32(r0, BITS);
				r1 = _mm_srai_epi32(r1, BITS);
				v0 = _mm_packus_epi16(_mm_packs_epi32(_mm_slli_si128(r0, 4), r1), z); // 0 B0 G0 R0 B1 G1 R1 0

				// process pixels 2 & 3
				t0 = _mm_madd_epi16(v1, m0); // a0 b0 a1 b1
				t1 = _mm_madd_epi16(v1, m1); // c0 d0 c1 d1
				t2 = _mm_madd_epi16(v1, m2); // e0 f0 e1 f1
				v1 = _mm_unpacklo_epi32(t0, t1); // a0 c0 b0 d0
				t0 = _mm_unpackhi_epi32(t0, t1); // a1 b1 c1 d1
				t1 = _mm_unpacklo_epi32(t2, z);  // e0 0 f0 0
				t2 = _mm_unpackhi_epi32(t2, z);  // e1 0 f1 0
				r0 = _mm_add_epi32(_mm_add_epi32(_mm_unpacklo_epi64(v1, t1), _mm_unpackhi_epi64(v1, t1)), m3); // B2 G2 R2 0
				r1 = _mm_add_epi32(_mm_add_epi32(_mm_unpacklo_epi64(t0, t2), _mm_unpackhi_epi64(t0, t2)), m3); // B3 G3 R3 0
				r0 = _mm_srai_epi32(r0, BITS);
				r1 = _mm_srai_epi32(r1, BITS);
				v1 = _mm_packus_epi16(_mm_packs_epi32(_mm_slli_si128(r0, 4), r1), z); // 0 B2 G2 R2 B3 G3 R3 0

				// process pixels 4 & 5
				t0 = _mm_madd_epi16(v2, m0); // a0 b0 a1 b1
				t1 = _mm_madd_epi16(v2, m1); // c0 d0 c1 d1
				t2 = _mm_madd_epi16(v2, m2); // e0 f0 e1 f1
				v2 = _mm_unpacklo_epi32(t0, t1); // a0 c0 b0 d0
				t0 = _mm_unpackhi_epi32(t0, t1); // a1 b1 c1 d1
				t1 = _mm_unpacklo_epi32(t2, z);  // e0 0 f0 0
				t2 = _mm_unpackhi_epi32(t2, z);  // e1 0 f1 0
				r0 = _mm_add_epi32(_mm_add_epi32(_mm_unpacklo_epi64(v2, t1), _mm_unpackhi_epi64(v2, t1)), m3); // B4 G4 R4 0
				r1 = _mm_add_epi32(_mm_add_epi32(_mm_unpacklo_epi64(t0, t2), _mm_unpackhi_epi64(t0, t2)), m3); // B5 G5 R5 0
				r0 = _mm_srai_epi32(r0, BITS);
				r1 = _mm_srai_epi32(r1, BITS);
				v2 = _mm_packus_epi16(_mm_packs_epi32(_mm_slli_si128(r0, 4), r1), z); // 0 B4 G4 R4 B5 G5 R5 0

				// process pixels 6 & 7
				t0 = _mm_madd_epi16(v3, m0); // a0 b0 a1 b1
				t1 = _mm_madd_epi16(v3, m1); // c0 d0 c1 d1
				t2 = _mm_madd_epi16(v3, m2); // e0 f0 e1 f1
				v3 = _mm_unpacklo_epi32(t0, t1); // a0 c0 b0 d0
				t0 = _mm_unpackhi_epi32(t0, t1); // a1 b1 c1 d1
				t1 = _mm_unpacklo_epi32(t2, z);  // e0 0 f0 0
				t2 = _mm_unpackhi_epi32(t2, z);  // e1 0 f1 0
				r0 = _mm_add_epi32(_mm_add_epi32(_mm_unpacklo_epi64(v3, t1), _mm_unpackhi_epi64(v3, t1)), m3); // B6 G6 R6 0
				r1 = _mm_add_epi32(_mm_add_epi32(_mm_unpacklo_epi64(t0, t2), _mm_unpackhi_epi64(t0, t2)), m3); // B7 G7 R7 0
				r0 = _mm_srai_epi32(r0, BITS);
				r1 = _mm_srai_epi32(r1, BITS);
				v3 = _mm_packus_epi16(_mm_packs_epi32(_mm_slli_si128(r0, 4), r1), z); // 0 B6 G6 R6 B7 G7 R7 0

				v0 = _mm_or_si128(_mm_srli_si128(v0, 1), _mm_slli_si128(v1, 5));
				v1 = _mm_or_si128(_mm_srli_si128(v1, 3), _mm_slli_si128(v2, 3));
				v2 = _mm_or_si128(_mm_srli_si128(v2, 5), _mm_slli_si128(v3, 1));
				_mm_storel_epi64((__m128i*)(dst + x), v0);
				_mm_storel_epi64((__m128i*)(dst + x + 8), v1);
				_mm_storel_epi64((__m128i*)(dst + x + 16), v2);
			}

			for ( ; x < size.width * 3; x += 3 ) {
				int v0 = src[x], v1 = src[x+1], v2 = src[x+2];
				uchar t0 = saturate_cast<uchar>((m00 * v0 + m01 * v1 + m02 * v2 + m03) >> BITS);
				uchar t1 = saturate_cast<uchar>((m10 * v0 + m11 * v1 + m12 * v2 + m13) >> BITS);
				uchar t2 = saturate_cast<uchar>((m20 * v0 + m21 * v1 + m22 * v2 + m23) >> BITS);
				dst[x] = t0; dst[x+1] = t1; dst[x+2] = t2;
			}
		}
		return;
	}

	for ( y = 0; y < size.height; y++ ) {
		const T* src = (const T*)(srcmat.data + srcmat.step * y);
		T* dst = (T*)(dstmat.data + dstmat.step * y);

		if ( dst_cn == 1 )
			for ( x = 0; x < size.width; x++, src += 3 )
				dst[x] = saturate_cast<T>(m[0] * CV_8TO32F(src[0]) +
										  m[1] * CV_8TO32F(src[1]) + m[2] * CV_8TO32F(src[2]) + m[3]);
		else {
			const WT* _m = m;
			for ( k = 0; k < dst_cn; k++, dst++, _m += 4 )
				for ( x = 0; x < size.width; x++ )
					dst[x* dst_cn] = saturate_cast<T>(_m[0] * CV_8TO32F(src[x*3]) +
													  _m[1] * CV_8TO32F(src[x*3+1]) + _m[2] * CV_8TO32F(src[x*3+2]) + _m[3]);
		}
	}
}

template<> void
transformC3_<ushort, float>( const Mat& srcmat, Mat& dstmat, Mat& tmat ) {
	typedef ushort T;
	typedef float WT;
	Size size = getContinuousSize( srcmat, dstmat );
	const float* m = (const float*)tmat.data;
	int dst_cn = dstmat.channels();
	int x, y, k;

	if ( checkHardwareSupport(CV_CPU_SSE2) && dst_cn == 3 ) {
		__m128 m0, m1, m2, m3;
		__m128i delta = _mm_setr_epi16(0, -32768, -32768, -32768, -32768, -32768, -32768, 0);
		load3x3Matrix(m, m0, m1, m2, m3);
		m3 = _mm_sub_ps(m3, _mm_setr_ps(32768.f, 32768.f, 32768.f, 0.f));

		for ( y = 0; y < size.height; y++ ) {
			const T* src = (const T*)(srcmat.data + srcmat.step * y);
			T* dst = (T*)(dstmat.data + dstmat.step * y);
			int x = 0;

			for ( ; x <= (size.width - 4) * 3; x += 4 * 3 ) {
				__m128i z = _mm_setzero_si128();
				__m128i v0 = _mm_loadu_si128((const __m128i*)(src + x)), v1;
				__m128i v2 = _mm_loadl_epi64((const __m128i*)(src + x + 8)), v3;
				v1 = _mm_unpacklo_epi16(_mm_srli_si128(v0, 6), z); // b1 g1 r1
				v3 = _mm_unpacklo_epi16(_mm_srli_si128(v2, 2), z); // b3 g3 r3
				v2 = _mm_or_si128(_mm_srli_si128(v0, 12), _mm_slli_si128(v2, 4));
				v0 = _mm_unpacklo_epi16(v0, z); // b0 g0 r0
				v2 = _mm_unpacklo_epi16(v2, z); // b2 g2 r2
				__m128 x0 = _mm_cvtepi32_ps(v0), x1 = _mm_cvtepi32_ps(v1);
				__m128 x2 = _mm_cvtepi32_ps(v2), x3 = _mm_cvtepi32_ps(v3);
				__m128 y0 = _mm_add_ps(_mm_add_ps(_mm_add_ps(
													  _mm_mul_ps(m0, _mm_shuffle_ps(x0, x0, _MM_SHUFFLE(0, 0, 0, 0))),
													  _mm_mul_ps(m1, _mm_shuffle_ps(x0, x0, _MM_SHUFFLE(1, 1, 1, 1)))),
												  _mm_mul_ps(m2, _mm_shuffle_ps(x0, x0, _MM_SHUFFLE(2, 2, 2, 2)))), m3);
				__m128 y1 = _mm_add_ps(_mm_add_ps(_mm_add_ps(
													  _mm_mul_ps(m0, _mm_shuffle_ps(x1, x1, _MM_SHUFFLE(0, 0, 0, 0))),
													  _mm_mul_ps(m1, _mm_shuffle_ps(x1, x1, _MM_SHUFFLE(1, 1, 1, 1)))),
												  _mm_mul_ps(m2, _mm_shuffle_ps(x1, x1, _MM_SHUFFLE(2, 2, 2, 2)))), m3);
				__m128 y2 = _mm_add_ps(_mm_add_ps(_mm_add_ps(
													  _mm_mul_ps(m0, _mm_shuffle_ps(x2, x2, _MM_SHUFFLE(0, 0, 0, 0))),
													  _mm_mul_ps(m1, _mm_shuffle_ps(x2, x2, _MM_SHUFFLE(1, 1, 1, 1)))),
												  _mm_mul_ps(m2, _mm_shuffle_ps(x2, x2, _MM_SHUFFLE(2, 2, 2, 2)))), m3);
				__m128 y3 = _mm_add_ps(_mm_add_ps(_mm_add_ps(
													  _mm_mul_ps(m0, _mm_shuffle_ps(x3, x3, _MM_SHUFFLE(0, 0, 0, 0))),
													  _mm_mul_ps(m1, _mm_shuffle_ps(x3, x3, _MM_SHUFFLE(1, 1, 1, 1)))),
												  _mm_mul_ps(m2, _mm_shuffle_ps(x3, x3, _MM_SHUFFLE(2, 2, 2, 2)))), m3);
				v0 = _mm_cvtps_epi32(y0); v1 = _mm_cvtps_epi32(y1);
				v2 = _mm_cvtps_epi32(y2); v3 = _mm_cvtps_epi32(y3);

				v0 = _mm_add_epi16(_mm_packs_epi32(_mm_slli_si128(v0, 4), v1), delta); // 0 b0 g0 r0 b1 g1 r1 0
				v2 = _mm_add_epi16(_mm_packs_epi32(_mm_slli_si128(v2, 4), v3), delta); // 0 b2 g2 r2 b3 g3 r3 0
				v1 = _mm_or_si128(_mm_srli_si128(v0, 2), _mm_slli_si128(v2, 10)); // b0 g0 r0 b1 g1 r1 b2 g2
				v2 = _mm_srli_si128(v2, 6); // r2 b3 g3 r3 0 0 0 0
				_mm_storeu_si128((__m128i*)(dst + x), v1);
				_mm_storel_epi64((__m128i*)(dst + x + 8), v2);
			}

			for ( ; x < size.width * 3; x += 3 ) {
				WT v0 = src[x], v1 = src[x+1], v2 = src[x+2];
				T t0 = saturate_cast<T>(m[0] * v0 + m[1] * v1 + m[2] * v2 + m[3]);
				T t1 = saturate_cast<T>(m[4] * v0 + m[5] * v1 + m[6] * v2 + m[7]);
				T t2 = saturate_cast<T>(m[8] * v0 + m[9] * v1 + m[10] * v2 + m[11]);
				dst[x] = t0; dst[x+1] = t1; dst[x+2] = t2;
			}
		}
		return;
	}

	for ( y = 0; y < size.height; y++ ) {
		const T* src = (const T*)(srcmat.data + srcmat.step * y);
		T* dst = (T*)(dstmat.data + dstmat.step * y);

		if ( dst_cn == 1 )
			for ( x = 0; x < size.width; x++, src += 3 ) {
				dst[x] = saturate_cast<T>(m[0] * src[0] + m[1] * src[1] + m[2] * src[2] + m[3]);
			}
		else {
			const WT* _m = m;
			for ( k = 0; k < dst_cn; k++, dst++, _m += 4 )
				for ( x = 0; x < size.width; x++ ) {
					dst[x* dst_cn] = saturate_cast<T>(_m[0] * src[x*3] + _m[1] * src[x*3+1] + _m[2] * src[x*3+2] + _m[3]);
				}
		}
	}
}

template<> void
transformC3_<float, float>( const Mat& srcmat, Mat& dstmat, Mat& tmat ) {
	typedef float T;
	typedef float WT;
	Size size = getContinuousSize( srcmat, dstmat );
	const float* m = (const float*)tmat.data;
	int dst_cn = dstmat.channels();
	int x, y, k;

	if ( checkHardwareSupport(CV_CPU_SSE2) && dst_cn == 3 ) {
		__m128 m0, m1, m2, m3;
		load3x3Matrix(m, m0, m1, m2, m3);

		for ( y = 0; y < size.height; y++ ) {
			const T* src = (const T*)(srcmat.data + srcmat.step * y);
			T* dst = (T*)(dstmat.data + dstmat.step * y);
			int x = 0;

			for ( ; x < (size.width - 1) * 3; x += 3 ) {
				__m128 x0 = _mm_loadu_ps(src + x);
				__m128 y0 = _mm_add_ps(_mm_add_ps(_mm_add_ps(
													  _mm_mul_ps(m0, _mm_shuffle_ps(x0, x0, _MM_SHUFFLE(0, 0, 0, 0))),
													  _mm_mul_ps(m1, _mm_shuffle_ps(x0, x0, _MM_SHUFFLE(1, 1, 1, 1)))),
												  _mm_mul_ps(m2, _mm_shuffle_ps(x0, x0, _MM_SHUFFLE(2, 2, 2, 2)))), m3);
				_mm_storel_pi((__m64*)(dst + x), y0);
				_mm_store_ss(dst + x + 2, _mm_movehl_ps(y0, y0));
			}

			for ( ; x < size.width * 3; x += 3 ) {
				WT v0 = src[x], v1 = src[x+1], v2 = src[x+2];
				T t0 = saturate_cast<T>(m[0] * v0 + m[1] * v1 + m[2] * v2 + m[3]);
				T t1 = saturate_cast<T>(m[4] * v0 + m[5] * v1 + m[6] * v2 + m[7]);
				T t2 = saturate_cast<T>(m[8] * v0 + m[9] * v1 + m[10] * v2 + m[11]);
				dst[x] = t0; dst[x+1] = t1; dst[x+2] = t2;
			}
		}
		return;
	}

	for ( y = 0; y < size.height; y++ ) {
		const T* src = (const T*)(srcmat.data + srcmat.step * y);
		T* dst = (T*)(dstmat.data + dstmat.step * y);

		if ( dst_cn == 1 )
			for ( x = 0; x < size.width; x++, src += 3 ) {
				dst[x] = saturate_cast<T>(m[0] * src[0] + m[1] * src[1] + m[2] * src[2] + m[3]);
			}
		else {
			const WT* _m = m;
			for ( k = 0; k < dst_cn; k++, dst++, _m += 4 )
				for ( x = 0; x < size.width; x++ ) {
					dst[x* dst_cn] = saturate_cast<T>(_m[0] * src[x*3] + _m[1] * src[x*3+1] + _m[2] * src[x*3+2] + _m[3]);
				}
		}
	}
}


template<> void
transformC4_<float, float>( const Mat& srcmat, Mat& dstmat, Mat& tmat ) {
	typedef float T;
	typedef float WT;
	Size size = getContinuousSize( srcmat, dstmat );
	const WT* m = (const WT*)tmat.data;
	int dst_cn = dstmat.channels();
	int x, y, k;

	if ( checkHardwareSupport(CV_CPU_SSE2) && dst_cn == 4 ) {
		__m128 m0, m1, m2, m3, m4;
		load4x4Matrix(m, m0, m1, m2, m3, m4);

		for ( y = 0; y < size.height; y++ ) {
			const T* src = (const T*)(srcmat.data + srcmat.step * y);
			T* dst = (T*)(dstmat.data + dstmat.step * y);
			for ( x = 0; x < size.width * 4; x += 4 ) {
				__m128 x0 = _mm_loadu_ps(src + x);
				__m128 y0 = _mm_add_ps(_mm_add_ps(_mm_add_ps(_mm_add_ps(
													  _mm_mul_ps(m0, _mm_shuffle_ps(x0, x0, _MM_SHUFFLE(0, 0, 0, 0))),
													  _mm_mul_ps(m1, _mm_shuffle_ps(x0, x0, _MM_SHUFFLE(1, 1, 1, 1)))),
												  _mm_mul_ps(m2, _mm_shuffle_ps(x0, x0, _MM_SHUFFLE(2, 2, 2, 2)))),
												  _mm_mul_ps(m3, _mm_shuffle_ps(x0, x0, _MM_SHUFFLE(3, 3, 3, 3)))), m4);
				_mm_storeu_ps(dst + x, y0);
			}
		}
		return;
	}

	for ( y = 0; y < size.height; y++ ) {
		const T* src = (const T*)(srcmat.data + srcmat.step * y);
		T* dst = (T*)(dstmat.data + dstmat.step * y);
		const WT* _m = m;
		for ( k = 0; k < dst_cn; k++, dst++, _m += 5 )
			for ( x = 0; x < size.width; x++ )
				dst[x* dst_cn] = saturate_cast<T>(_m[0] * src[x*4] + _m[1] * src[x*4+1] +
												  _m[2] * src[x*4+2] + _m[3] * src[x*4+3] + _m[4]);
	}
}

#endif


template<typename T, typename WT> static void
diagtransC2_( const Mat& srcmat, Mat& dstmat, Mat& tmat ) {
	Size size = getContinuousSize( srcmat, dstmat );
	const WT* m = (const WT*)tmat.data;
	int x, y;

	for ( y = 0; y < size.height; y++ ) {
		const T* src = (const T*)(srcmat.data + srcmat.step * y);
		T* dst = (T*)(dstmat.data + dstmat.step * y);

		for ( x = 0; x < size.width * 2; x += 2 ) {
			T t0 = saturate_cast<T>(m[0] * src[x] + m[2]);
			T t1 = saturate_cast<T>(m[4] * src[x+1] + m[5]);
			dst[x] = t0; dst[x+1] = t1;
		}
	}
}

template<typename T, typename WT> static void
diagtransC3_( const Mat& srcmat, Mat& dstmat, Mat& tmat ) {
	Size size = getContinuousSize( srcmat, dstmat );
	const WT* m = (const WT*)tmat.data;
	int x, y;

	for ( y = 0; y < size.height; y++ ) {
		const T* src = (const T*)(srcmat.data + srcmat.step * y);
		T* dst = (T*)(dstmat.data + dstmat.step * y);

		for ( x = 0; x < size.width * 3; x += 3 ) {
			T t0 = saturate_cast<T>(m[0] * src[x] + m[3]);
			T t1 = saturate_cast<T>(m[5] * src[x+1] + m[7]);
			T t2 = saturate_cast<T>(m[10] * src[x+2] + m[11]);
			dst[x] = t0; dst[x+1] = t1; dst[x+2] = t2;
		}
	}
}

template<typename T, typename WT> static void
diagtransC4_( const Mat& srcmat, Mat& dstmat, Mat& tmat ) {
	Size size = getContinuousSize( srcmat, dstmat );
	const WT* m = (const WT*)tmat.data;
	int x, y;

	for ( y = 0; y < size.height; y++ ) {
		const T* src = (const T*)(srcmat.data + srcmat.step * y);
		T* dst = (T*)(dstmat.data + dstmat.step * y);

		for ( x = 0; x < size.width * 4; x += 4 ) {
			T t0 = saturate_cast<T>(m[0] * src[x] + m[4]);
			T t1 = saturate_cast<T>(m[6] * src[x+1] + m[9]);
			dst[x] = t0; dst[x+1] = t1;
			t0 = saturate_cast<T>(m[12] * src[x+2] + m[14]);
			t1 = saturate_cast<T>(m[18] * src[x+3] + m[19]);
			dst[x+2] = t0; dst[x+3] = t1;
		}
	}
}

typedef void (*TransformFunc)( const Mat& src, Mat& dst, Mat& M );

void transform( const Mat& src, Mat& dst, const Mat& _m ) {
	static TransformFunc tab[2][32] = {
		{
			transformC1_<uchar, float>, 0, transformC1_<ushort, float>, transformC1_<short, float>,
			transformC1_<int, double>, transformC1_<float, float>, transformC1_<double, double>, 0,
			transformC2_<uchar, float>, 0, transformC2_<ushort, float>, transformC2_<short, float>,
			transformC2_<int, double>, transformC2_<float, float>, transformC2_<double, double>, 0,
			transformC3_<uchar, float>, 0, transformC3_<ushort, float>, transformC3_<short, float>,
			transformC3_<int, double>, transformC3_<float, float>, transformC3_<double, double>, 0,
			transformC4_<uchar, float>, 0, transformC4_<ushort, float>, transformC4_<short, float>,
			transformC4_<int, double>, transformC4_<float, float>, transformC4_<double, double>, 0
		},

		{
			0, 0, 0, 0, 0, 0, 0, 0,
			0, 0, diagtransC2_<ushort, float>, diagtransC2_<short, float>,
			diagtransC2_<int, double>, diagtransC2_<float, float>, diagtransC2_<double, double>, 0,
			0, 0, diagtransC3_<ushort, float>, diagtransC3_<short, float>,
			diagtransC3_<int, double>, diagtransC3_<float, float>, diagtransC3_<double, double>, 0,
			0, 0, diagtransC4_<ushort, float>, diagtransC4_<short, float>,
			diagtransC4_<int, double>, diagtransC4_<float, float>, diagtransC4_<double, double>, 0
		}
	};

	int type = src.type(), depth = src.depth(), scn = src.channels(), dcn = _m.rows;
	bool isDiag = false;
	CV_Assert( (scn == _m.cols || scn + 1 == _m.cols) && scn <= 4 && dcn <= 4 );

	double mbuf[20] = {0};
	Mat m = _m;

	dst.create( src.size(), CV_MAKETYPE(depth, dcn) );
	Size size = getContinuousSize( src, dst );

	int mtype = depth == CV_32S || depth == CV_64F ? CV_64F : CV_32F;
	if ( !_m.isContinuous() || _m.type() != mtype || _m.cols != scn + 1 ) {
		m = Mat(dcn, scn + 1, mtype, mbuf);
		Mat tmat_part = m.colRange(0, _m.cols);
		_m.convertTo(tmat_part, mtype);
	}

	if ( scn == dcn ) {
		int i, j;
		double eps = mtype == CV_32F ? FLT_EPSILON : DBL_EPSILON;

		if ( scn == 1 ) {
			double alpha, beta;
			if ( mtype == CV_32F ) {
				alpha = ((float*)m.data)[0], beta = ((float*)m.data)[1];
			} else {
				alpha = ((double*)m.data)[0], beta = ((double*)m.data)[1];
			}
			src.convertTo( dst, dst.type(), alpha, beta );
			return;
		}

		for ( i = 0, isDiag = true; isDiag && i < scn; i++ ) {
			for ( j = 0; isDiag && j < scn; j++ ) {
				double v = mtype == CV_32F ? ((float*)m.data)[i*(scn+1)+j] :
						   ((double*)m.data)[i*(scn+1)+j];
				if ( i != j && fabs(v) > eps ) {
					isDiag = false;
				}
			}
		}

		if ( isDiag && depth == CV_8U ) {
			Mat lut(1, 256, CV_8UC(scn));
			for ( i = 0; i < scn; i++ ) {
				uchar* data = lut.data + i;
				double val, delta;
				if ( mtype == CV_32F ) {
					val = ((float*)m.data)[i*(scn+1) + scn];
					delta = ((float*)m.data)[i*(scn+1) + i];
				} else {
					val = ((double*)m.data)[i*(scn+1) + scn];
					delta = ((double*)m.data)[i*(scn+1) + i];
				}
				for ( j = 0; j < 256; j++, val += delta ) {
					int ival = cvRound(val);
					data[j* scn] = CV_CAST_8U(ival);
				}
			}
			LUT( src, lut, dst );
			return;
		}
	}

	TransformFunc func = tab[isDiag][type];
	CV_Assert( func != 0 );
	func( src, dst, m );
}


/****************************************************************************************\
*                                  Perspective Transform                                 *
\****************************************************************************************/

template<typename T> static void
perspectiveTransform2_( const Mat& srcmat, Mat& dstmat, const double* mat ) {
	Size size = getContinuousSize( srcmat, dstmat, srcmat.channels() );

	for ( int i = 0; i < size.height; i++ ) {
		const T* src = (const T*)(srcmat.data + srcmat.step * i);
		T* dst = (T*)(dstmat.data + dstmat.step * i);

		for ( int j = 0; j < size.width; j += 2 ) {
			T x = src[j], y = src[j + 1];
			double w = x * mat[6] + y * mat[7] + mat[8];

			if ( fabs(w) > FLT_EPSILON ) {
				w = 1. / w;
				dst[j] = (T)((x * mat[0] + y * mat[1] + mat[2]) * w);
				dst[j+1] = (T)((x * mat[3] + y * mat[4] + mat[5]) * w);
			} else {
				dst[j] = dst[j+1] = (T)0;
			}
		}
	}
}

template<typename T> static void
perspectiveTransform3_( const Mat& srcmat, Mat& dstmat, const double* mat ) {
	Size size = getContinuousSize( srcmat, dstmat, srcmat.channels() );

	for ( int i = 0; i < size.height; i++ ) {
		const T* src = (const T*)(srcmat.data + srcmat.step * i);
		T* dst = (T*)(dstmat.data + dstmat.step * i);

		for ( int j = 0; j < size.width; j += 3 ) {
			T x = src[j], y = src[j + 1], z = src[j + 2];
			double w = x * mat[12] + y * mat[13] + z * mat[14] + mat[15];

			if ( fabs(w) > FLT_EPSILON ) {
				w = 1. / w;
				dst[j] = (T)((x * mat[0] + y * mat[1] + z * mat[2] + mat[3]) * w);
				dst[j+1] = (T)((x * mat[4] + y * mat[5] + z * mat[6] + mat[7]) * w);
				dst[j+2] = (T)((x * mat[8] + y * mat[9] + z * mat[10] + mat[11]) * w);
			} else {
				dst[j] = dst[j+1] = dst[j+2] = (T)0;
			}
		}
	}
}

template<typename T> static void
perspectiveTransform23_( const Mat& srcmat, Mat& dstmat, const double* mat ) {
	Size size = getContinuousSize( srcmat, dstmat, srcmat.channels() );

	for ( int i = 0; i < size.height; i++ ) {
		const T* src = (const T*)(srcmat.data + srcmat.step * i);
		T* dst = (T*)(dstmat.data + dstmat.step * i);

		for ( int j = 0; j < size.width; j++, src += 3, dst += 2 ) {
			T x = src[0], y = src[1], z = src[2];
			double w = x * mat[8] + y * mat[9] + z * mat[10] + mat[11];

			if ( fabs(w) > FLT_EPSILON ) {
				w = 1. / w;
				dst[0] = (T)((x * mat[0] + y * mat[1] + z * mat[2] + mat[3]) * w);
				dst[1] = (T)((x * mat[4] + y * mat[5] + z * mat[6] + mat[7]) * w);
			} else {
				dst[0] = dst[1] = (T)0;
			}
		}
	}
}

typedef void (*PerspectiveTransformFunc)(const Mat& src, Mat& dst, const double* mat );

void perspectiveTransform( const Mat& src, Mat& dst, const Mat& _m ) {
	int depth = src.depth(), scn = src.channels(), dcn = _m.rows - 1;
	CV_Assert( (depth == CV_32F || depth == CV_64F) && scn + 1 == _m.cols && scn <= 4 &&
			   ((scn == 2 && dcn == 2) || (scn == 3 && dcn == 3) || (scn == 2 && dcn == 3)));

	double mbuf[16] = {0};
	Mat tmat;
	const double* m = (const double*)_m.data;

	dst.create( src.size(), CV_MAKETYPE(depth, dcn) );

	if ( !_m.isContinuous() || _m.type() != CV_64F ) {
		tmat = Mat(dcn + 1, scn + 1, CV_64F, mbuf);
		_m.convertTo(tmat, CV_64F);
		m = (const double*)tmat.data;
	}

	PerspectiveTransformFunc func = 0;
	if ( scn == 2 && dcn == 2 ) {
		if (depth == CV_32F) {
			func = perspectiveTransform2_<float>;
		} else {
			func = perspectiveTransform2_<double>;
		}
	} else if ( scn == 2 && dcn == 3 ) {
		if (depth == CV_32F) {
			func = perspectiveTransform23_<float>;
		} else {
			func = perspectiveTransform23_<double>;
		}
	} else if ( scn == 3 && dcn == 3 ) {
		if (depth == CV_32F) {
			func = perspectiveTransform3_<float>;
		} else {
			func = perspectiveTransform3_<double>;
		}
	} else {
		CV_Error( CV_StsNotImplemented, "Only 2->2, 2->3 and 3->3 perspective transformation is implemented" );
	}
	func( src, dst, m );
}

/****************************************************************************************\
*                                       ScaleAdd                                         *
\****************************************************************************************/

void scaleAdd( const Mat& src1, double alpha, const Mat& src2, Mat& dst ) {
	int type = src1.type(), depth = CV_MAT_DEPTH(type);
	CV_Assert( src1.size() == src2.size() && type == src2.type() );
	dst.create( src1.size(), type );
	Size size = getContinuousSize( src1, src2, dst, src1.channels() );

	if ( depth == CV_32F ) {
		const float* s1 = (const float*)src1.data, *s2 = (const float*)src2.data;
		float* d = (float*)dst.data;
		size_t step1 = src1.step / sizeof(s1[0]), step2 = src2.step / sizeof(s2[0]);
		size_t step = dst.step / sizeof(d[0]);
		float a = (float)alpha;

		if ( size.width == 1 ) {
			for ( ; size.height--; s1 += step1, s2 += step2, d += step ) {
				d[0] = s1[0] * a + s2[0];
			}
			return;
		}

		for ( ; size.height--; s1 += step1, s2 += step2, d += step ) {
			int i;
			for ( i = 0; i <= size.width - 4; i += 4 ) {
				float t0 = s1[i] * a + s2[i];
				float t1 = s1[i+1] * a + s2[i+1];
				d[i] = t0;
				d[i+1] = t1;
				t0 = s1[i+2] * a + s2[i+2];
				t1 = s1[i+3] * a + s2[i+3];
				d[i+2] = t0;
				d[i+3] = t1;
			}

			for ( ; i < size.width; i++ ) {
				d[i] = s1[i] * a + s2[i];
			}
		}
	} else if ( depth == CV_64F ) {
		const double* s1 = (const double*)src1.data, *s2 = (const double*)src2.data;
		double* d = (double*)dst.data;
		size_t step1 = src1.step / sizeof(s1[0]), step2 = src2.step / sizeof(s2[0]);
		size_t step = dst.step / sizeof(d[0]);

		if ( size.width == 1 ) {
			for ( ; size.height--; s1 += step1, s2 += step2, d += step ) {
				d[0] = s1[0] * alpha + s2[0];
			}
			return;
		}

		for ( ; size.height--; s1 += step1, s2 += step2, d += step ) {
			int i;
			for ( i = 0; i <= size.width - 4; i += 4 ) {
				double t0 = s1[i] * alpha + s2[i];
				double t1 = s1[i+1] * alpha + s2[i+1];
				d[i] = t0;
				d[i+1] = t1;
				t0 = s1[i+2] * alpha + s2[i+2];
				t1 = s1[i+3] * alpha + s2[i+3];
				d[i+2] = t0;
				d[i+3] = t1;
			}

			for ( ; i < size.width; i++ ) {
				d[i] = s1[i] * alpha + s2[i];
			}
		}
	} else {
		addWeighted(src1, alpha, src2, 1, 0, dst);
	}
}

/****************************************************************************************\
*                                 Covariation Matrix                                     *
\****************************************************************************************/

void calcCovarMatrix( const Mat* data, int nsamples, Mat& covar, Mat& _mean, int flags, int ctype ) {
	CV_Assert( data && nsamples > 0 );
	Size size = data[0].size();
	int sz = size.width * size.height, esz = (int)data[0].elemSize();
	int type = data[0].type();
	Mat mean;
	ctype = std::max(std::max(CV_MAT_DEPTH(ctype >= 0 ? ctype : type), _mean.depth()), CV_32F);

	if ( (flags & CV_COVAR_USE_AVG) != 0 ) {
		CV_Assert( _mean.size() == size );
		if ( _mean.isContinuous() && _mean.type() == ctype ) {
			mean = _mean.reshape(1, 1);
		} else {
			_mean.convertTo(mean, ctype);
			mean = mean.reshape(1, 1);
		}
	}

	Mat _data(nsamples, sz, type);
	for ( int i = 0; i < nsamples; i++ ) {
		CV_Assert( data[i].size() == size && data[i].type() == type );
		if ( data[i].isContinuous() ) {
			memcpy( _data.ptr(i), data[i].data, sz * esz );
		} else {
			Mat dataRow(size.height, size.width, type, _data.ptr(i));
			data[i].copyTo(dataRow);
		}
	}

	calcCovarMatrix( _data, covar, mean, (flags & ~(CV_COVAR_ROWS | CV_COVAR_COLS)) | CV_COVAR_ROWS, ctype );
	if ( (flags & CV_COVAR_USE_AVG) == 0 ) {
		_mean = mean.reshape(1, size.height);
	}
}

void calcCovarMatrix( const Mat& data, Mat& covar, Mat& _mean, int flags, int ctype ) {
	CV_Assert( ((flags & CV_COVAR_ROWS) != 0) ^ ((flags & CV_COVAR_COLS) != 0) );
	bool takeRows = (flags & CV_COVAR_ROWS) != 0;
	int type = data.type();
	int nsamples = takeRows ? data.rows : data.cols;
	CV_Assert( nsamples > 0 );
	Size size = takeRows ? Size(data.cols, 1) : Size(1, data.rows);
	Mat mean = _mean;
	ctype = std::max(std::max(CV_MAT_DEPTH(ctype >= 0 ? ctype : type), _mean.depth()), CV_32F);

	if ( (flags & CV_COVAR_USE_AVG) != 0 ) {
		CV_Assert( mean.size() == size );
		if ( mean.type() != ctype ) {
			_mean.convertTo(mean, ctype);
		}
	} else {
		reduce( data, _mean, takeRows ? 0 : 1, CV_REDUCE_AVG, ctype );
		mean = _mean;
	}

	mulTransposed( data, covar, ((flags & CV_COVAR_NORMAL) == 0) ^ takeRows,
				   mean, (flags & CV_COVAR_SCALE) != 0 ? 1. / nsamples : 1, ctype );
}

/****************************************************************************************\
*                                        Mahalanobis                                     *
\****************************************************************************************/

double Mahalanobis( const Mat& v1, const Mat& v2, const Mat& icovar ) {
	int type = v1.type(), depth = v1.depth();
	Size sz = v1.size();
	int i, j, len = sz.width * sz.height * v1.channels();
	AutoBuffer<uchar> buf(len * v1.elemSize());
	double result = 0;

	CV_Assert( type == v2.type() && type == icovar.type() &&
			   sz == v2.size() && len == icovar.rows && len == icovar.cols );

	if ( v1.isContinuous() && v2.isContinuous() ) {
		sz.width *= sz.height;
		sz.height = 1;
	}

	if ( depth == CV_32F ) {
		const float* src1 = (const float*)v1.data;
		const float* src2 = (const float*)v2.data;
		size_t step1 = v1.step / sizeof(src1[0]);
		size_t step2 = v2.step / sizeof(src2[0]);
		float* diff = (float*)(uchar*)buf;
		const float* mat = (const float*)icovar.data;
		size_t matstep = icovar.step / sizeof(mat[0]);

		for ( ; sz.height--; src1 += step1, src2 += step2, diff += sz.width ) {
			for ( i = 0; i < sz.width; i++ ) {
				diff[i] = src1[i] - src2[i];
			}
		}

		diff = (float*)(uchar*)buf;
		for ( i = 0; i < len; i++, mat += matstep ) {
			double row_sum = 0;
			for ( j = 0; j <= len - 4; j += 4 )
				row_sum += diff[j] * mat[j] + diff[j+1] * mat[j+1] +
						   diff[j+2] * mat[j+2] + diff[j+3] * mat[j+3];
			for ( ; j < len; j++ ) {
				row_sum += diff[j] * mat[j];
			}
			result += row_sum * diff[i];
		}
	} else if ( depth == CV_64F ) {
		const double* src1 = (const double*)v1.data;
		const double* src2 = (const double*)v2.data;
		size_t step1 = v1.step / sizeof(src1[0]);
		size_t step2 = v2.step / sizeof(src2[0]);
		double* diff = (double*)(uchar*)buf;
		const double* mat = (const double*)icovar.data;
		size_t matstep = icovar.step / sizeof(mat[0]);

		for ( ; sz.height--; src1 += step1, src2 += step2, diff += sz.width ) {
			for ( i = 0; i < sz.width; i++ ) {
				diff[i] = src1[i] - src2[i];
			}
		}

		diff = (double*)(uchar*)buf;
		for ( i = 0; i < len; i++, mat += matstep ) {
			double row_sum = 0;
			for ( j = 0; j <= len - 4; j += 4 )
				row_sum += diff[j] * mat[j] + diff[j+1] * mat[j+1] +
						   diff[j+2] * mat[j+2] + diff[j+3] * mat[j+3];
			for ( ; j < len; j++ ) {
				row_sum += diff[j] * mat[j];
			}
			result += row_sum * diff[i];
		}
	} else {
		CV_Error( CV_StsUnsupportedFormat, "" );
	}

	return std::sqrt(result);
}


/****************************************************************************************\
*                                        cvMulTransposed                                 *
\****************************************************************************************/

template<typename sT, typename dT> static void
MulTransposedR( const Mat& srcmat, Mat& dstmat, const Mat& deltamat, double scale ) {
	int i, j, k;
	const sT* src = (const sT*)srcmat.data;
	dT* dst = (dT*)dstmat.data;
	const dT* delta = (const dT*)deltamat.data;
	size_t srcstep = srcmat.step / sizeof(src[0]);
	size_t dststep = dstmat.step / sizeof(dst[0]);
	size_t deltastep = deltamat.rows > 1 ? deltamat.step / sizeof(delta[0]) : 0;
	int delta_cols = deltamat.cols;
	Size size = srcmat.size();
	dT* tdst = dst;
	dT* col_buf = 0;
	dT* delta_buf = 0;
	int buf_size = size.height * sizeof(dT);
	AutoBuffer<uchar> buf;

	if ( delta && delta_cols < size.width ) {
		assert( delta_cols == 1 );
		buf_size *= 5;
	}
	buf.allocate(buf_size);
	col_buf = (dT*)(uchar*)buf;

	if ( delta && delta_cols < size.width ) {
		delta_buf = col_buf + size.height;
		for ( i = 0; i < size.height; i++ )
			delta_buf[i*4] = delta_buf[i*4+1] =
								 delta_buf[i*4+2] = delta_buf[i*4+3] = delta[i*deltastep];
		delta = delta_buf;
		deltastep = deltastep ? 4 : 0;
	}

	if ( !delta )
		for ( i = 0; i < size.width; i++, tdst += dststep ) {
			for ( k = 0; k < size.height; k++ ) {
				col_buf[k] = src[k*srcstep+i];
			}

			for ( j = i; j <= size.width - 4; j += 4 ) {
				double s0 = 0, s1 = 0, s2 = 0, s3 = 0;
				const sT* tsrc = src + j;

				for ( k = 0; k < size.height; k++, tsrc += srcstep ) {
					double a = col_buf[k];
					s0 += a * tsrc[0];
					s1 += a * tsrc[1];
					s2 += a * tsrc[2];
					s3 += a * tsrc[3];
				}

				tdst[j] = (dT)(s0 * scale);
				tdst[j+1] = (dT)(s1 * scale);
				tdst[j+2] = (dT)(s2 * scale);
				tdst[j+3] = (dT)(s3 * scale);
			}

			for ( ; j < size.width; j++ ) {
				double s0 = 0;
				const sT* tsrc = src + j;

				for ( k = 0; k < size.height; k++, tsrc += srcstep ) {
					s0 += col_buf[k] * tsrc[0];
				}

				tdst[j] = (dT)(s0 * scale);
			}
		}
	else
		for ( i = 0; i < size.width; i++, tdst += dststep ) {
			if ( !delta_buf )
				for ( k = 0; k < size.height; k++ ) {
					col_buf[k] = src[k*srcstep+i] - delta[k*deltastep+i];
				}
			else
				for ( k = 0; k < size.height; k++ ) {
					col_buf[k] = src[k*srcstep+i] - delta_buf[k*deltastep];
				}

			for ( j = i; j <= size.width - 4; j += 4 ) {
				double s0 = 0, s1 = 0, s2 = 0, s3 = 0;
				const sT* tsrc = src + j;
				const dT* d = delta_buf ? delta_buf : delta + j;

				for ( k = 0; k < size.height; k++, tsrc += srcstep, d += deltastep ) {
					double a = col_buf[k];
					s0 += a * (tsrc[0] - d[0]);
					s1 += a * (tsrc[1] - d[1]);
					s2 += a * (tsrc[2] - d[2]);
					s3 += a * (tsrc[3] - d[3]);
				}

				tdst[j] = (dT)(s0 * scale);
				tdst[j+1] = (dT)(s1 * scale);
				tdst[j+2] = (dT)(s2 * scale);
				tdst[j+3] = (dT)(s3 * scale);
			}

			for ( ; j < size.width; j++ ) {
				double s0 = 0;
				const sT* tsrc = src + j;
				const dT* d = delta_buf ? delta_buf : delta + j;

				for ( k = 0; k < size.height; k++, tsrc += srcstep, d += deltastep ) {
					s0 += col_buf[k] * (tsrc[0] - d[0]);
				}

				tdst[j] = (dT)(s0 * scale);
			}
		}
}


template<typename sT, typename dT> static void
MulTransposedL( const Mat& srcmat, Mat& dstmat, const Mat& deltamat, double scale ) {
	int i, j, k;
	const sT* src = (const sT*)srcmat.data;
	dT* dst = (dT*)dstmat.data;
	const dT* delta = (const dT*)deltamat.data;
	size_t srcstep = srcmat.step / sizeof(src[0]);
	size_t dststep = dstmat.step / sizeof(dst[0]);
	size_t deltastep = deltamat.rows > 1 ? deltamat.step / sizeof(delta[0]) : 0;
	int delta_cols = deltamat.cols;
	Size size = srcmat.size();
	dT* tdst = dst;

	if ( !delta )
		for ( i = 0; i < size.height; i++, tdst += dststep )
			for ( j = i; j < size.height; j++ ) {
				double s = 0;
				const sT* tsrc1 = src + i * srcstep;
				const sT* tsrc2 = src + j * srcstep;

				for ( k = 0; k <= size.width - 4; k += 4 )
					s += tsrc1[k] * tsrc2[k] + tsrc1[k+1] * tsrc2[k+1] +
						 tsrc1[k+2] * tsrc2[k+2] + tsrc1[k+3] * tsrc2[k+3];
				for ( ; k < size.width; k++ ) {
					s += tsrc1[k] * tsrc2[k];
				}
				tdst[j] = (dT)(s * scale);
			}
	else {
		dT delta_buf[4];
		int delta_shift = delta_cols == size.width ? 4 : 0;
		AutoBuffer<uchar> buf(size.width * sizeof(dT));
		dT* row_buf = (dT*)(uchar*)buf;

		for ( i = 0; i < size.height; i++, tdst += dststep ) {
			const sT* tsrc1 = src + i * srcstep;
			const dT* tdelta1 = delta + i * deltastep;

			if ( delta_cols < size.width )
				for ( k = 0; k < size.width; k++ ) {
					row_buf[k] = tsrc1[k] - tdelta1[0];
				}
			else
				for ( k = 0; k < size.width; k++ ) {
					row_buf[k] = tsrc1[k] - tdelta1[k];
				}

			for ( j = i; j < size.height; j++ ) {
				double s = 0;
				const sT* tsrc2 = src + j * srcstep;
				const dT* tdelta2 = delta + j * deltastep;
				if ( delta_cols < size.width ) {
					delta_buf[0] = delta_buf[1] =
									   delta_buf[2] = delta_buf[3] = tdelta2[0];
					tdelta2 = delta_buf;
				}
				for ( k = 0; k <= size.width - 4; k += 4, tdelta2 += delta_shift )
					s += row_buf[k] * (tsrc2[k] - tdelta2[0]) +
						 row_buf[k+1] * (tsrc2[k+1] - tdelta2[1]) +
						 row_buf[k+2] * (tsrc2[k+2] - tdelta2[2]) +
						 row_buf[k+3] * (tsrc2[k+3] - tdelta2[3]);
				for ( ; k < size.width; k++, tdelta2++ ) {
					s += row_buf[k] * (tsrc2[k] - tdelta2[0]);
				}
				tdst[j] = (dT)(s * scale);
			}
		}
	}
}

typedef void (*MulTransposedFunc)(const Mat& src, Mat& dst, const Mat& delta, double scale);

void mulTransposed( const Mat& src, Mat& dst, bool ata,
					const Mat& _delta, double scale, int dtype ) {
	const int gemm_level = 100; // boundary above which GEMM is faster.
	int stype = src.type();
	Mat delta = _delta;
	dtype = std::max(std::max(CV_MAT_DEPTH(dtype >= 0 ? dtype : stype), delta.depth()), CV_32F);
	CV_Assert( src.channels() == 1 );

	if ( delta.data ) {
		CV_Assert( delta.channels() == 1 &&
				   (delta.rows == src.rows || delta.rows == 1) &&
				   (delta.cols == src.cols || delta.cols == 1));
		if ( delta.type() != dtype ) {
			_delta.convertTo(delta, dtype);
		}
	}

	int dsize = ata ? src.cols : src.rows;
	dst.create( dsize, dsize, dtype );

	if ( src.data == dst.data || (stype == dtype &&
								  (dst.cols >= gemm_level && dst.rows >= gemm_level &&
								   src.cols >= gemm_level && src.rows >= gemm_level))) {
		Mat src2;
		const Mat* tsrc = &src;
		if ( delta.data ) {
			if ( delta.size() == src.size() ) {
				subtract( src, delta, src2 );
			} else {
				repeat(delta, src.rows / delta.rows, src.cols / delta.cols, src2);
				subtract( src, src2, src2 );
			}
			tsrc = &src2;
		}
		gemm( *tsrc, *tsrc, scale, Mat(), 0, dst, ata ? GEMM_1_T : GEMM_2_T );
	} else {
		MulTransposedFunc func = 0;
		if (stype == CV_8U && dtype == CV_32F) {
			if (ata) {
				func = MulTransposedR<uchar, float>;
			} else {
				func = MulTransposedL<uchar, float>;
			}
		} else if (stype == CV_8U && dtype == CV_64F) {
			if (ata) {
				func = MulTransposedR<uchar, double>;
			} else {
				func = MulTransposedL<uchar, double>;
			}
		} else if (stype == CV_16U && dtype == CV_32F) {
			if (ata) {
				func = MulTransposedR<ushort, float>;
			} else {
				func = MulTransposedL<ushort, float>;
			}
		} else if (stype == CV_16U && dtype == CV_64F) {
			if (ata) {
				func = MulTransposedR<ushort, double>;
			} else {
				func = MulTransposedL<ushort, double>;
			}
		} else if (stype == CV_16S && dtype == CV_32F) {
			if (ata) {
				func = MulTransposedR<short, float>;
			} else {
				func = MulTransposedL<short, float>;
			}
		} else if (stype == CV_16S && dtype == CV_64F) {
			if (ata) {
				func = MulTransposedR<short, double>;
			} else {
				func = MulTransposedL<short, double>;
			}
		} else if (stype == CV_32F && dtype == CV_32F) {
			if (ata) {
				func = MulTransposedR<float, float>;
			} else {
				func = MulTransposedL<float, float>;
			}
		} else if (stype == CV_32F && dtype == CV_64F) {
			if (ata) {
				func = MulTransposedR<float, double>;
			} else {
				func = MulTransposedL<float, double>;
			}
		} else if (stype == CV_64F && dtype == CV_64F) {
			if (ata) {
				func = MulTransposedR<double, double>;
			} else {
				func = MulTransposedL<double, double>;
			}
		}
		if ( !func ) {
			CV_Error( CV_StsUnsupportedFormat, "" );
		}

		func( src, dst, delta, scale );
		completeSymm( dst, false );
	}
}

/****************************************************************************************\
*                                      Dot Product                                       *
\****************************************************************************************/

template<typename T, typename WT, typename ST> static double
dotprod_( const Mat& srcmat1, const Mat& srcmat2 ) {
	const T* src1 = (const T*)srcmat1.data, *src2 = (const T*)srcmat2.data;
	size_t step1 = srcmat1.step / sizeof(src1[0]), step2 = srcmat2.step / sizeof(src2[0]);
	ST sum = 0;
	Size size = getContinuousSize( srcmat1, srcmat2, srcmat1.channels() );

	if ( size.width == 1 ) {
		WT t = 0;
		for ( ; size.height--; src1 += step1, src2 += step2 ) {
			t += (WT)src1[0] * src2[0];
		}
		sum += t;
	} else {
		for ( ; size.height--; src1 += step1, src2 += step2 ) {
			int i;
			WT t = 0;
			for ( i = 0; i <= size.width - 4; i += 4 ) {
				sum += (WT)src1[i] * src2[i] +
					   (WT)src1[i+1] * src2[i+1] +
					   (WT)src1[i+2] * src2[i+2] +
					   (WT)src1[i+3] * src2[i+3];
			}

			for ( ; i < size.width; i++ ) {
				t += (WT)src1[i] * src2[i];
			}
			sum += t;
		}
	}
	return (double)sum;
}

typedef double (*DotProductFunc)(const Mat& src1, const Mat& src2);

double Mat::dot(const Mat& mat) const {
	static DotProductFunc tab[] = {
		dotprod_<uchar, int, int64>, 0,
		dotprod_<ushort, double, double>,
		dotprod_<short, double, double>,
		dotprod_<int, double, double>,
		dotprod_<float, double, double>,
		dotprod_<double, double, double>, 0
	};

	DotProductFunc func = tab[depth()];
	CV_Assert( mat.type() == type() && mat.size() == size() && func != 0 );
	return func( *this, mat );
}

/****************************************************************************************\
*                                          PCA                                           *
\****************************************************************************************/

PCA::PCA() {}

PCA::PCA(const Mat& data, const Mat& mean, int flags, int maxComponents) {
	operator()(data, mean, flags, maxComponents);
}

PCA& PCA::operator()(const Mat& data, const Mat& _mean, int flags, int maxComponents) {
	int covar_flags = CV_COVAR_SCALE;
	int i, len, in_count;
	Size mean_sz;

	CV_Assert( data.channels() == 1 );
	if ( flags & CV_PCA_DATA_AS_COL ) {
		len = data.rows;
		in_count = data.cols;
		covar_flags |= CV_COVAR_COLS;
		mean_sz = Size(1, len);
	} else {
		len = data.cols;
		in_count = data.rows;
		covar_flags |= CV_COVAR_ROWS;
		mean_sz = Size(len, 1);
	}

	int count = std::min(len, in_count), out_count = count;
	if ( maxComponents > 0 ) {
		out_count = std::min(count, maxComponents);
	}

	// "scrambled" way to compute PCA (when cols(A)>rows(A)):
	// B = A'A; B*x=b*x; C = AA'; C*y=c*y -> AA'*y=c*y -> A'A*(A'*y)=c*(A'*y) -> c = b, x=A'*y
	if ( len <= in_count ) {
		covar_flags |= CV_COVAR_NORMAL;
	}

	int ctype = std::max(CV_32F, data.depth());
	mean.create( mean_sz, ctype );

	Mat covar( count, count, ctype );

	if ( _mean.data ) {
		CV_Assert( _mean.size() == mean_sz );
		_mean.convertTo(mean, ctype);
	}

	calcCovarMatrix( data, covar, mean, covar_flags, ctype );
	eigen( covar, eigenvalues, eigenvectors );

	if ( !(covar_flags & CV_COVAR_NORMAL) ) {
		// CV_PCA_DATA_AS_ROW: cols(A)>rows(A). x=A'*y -> x'=y'*A
		// CV_PCA_DATA_AS_COL: rows(A)>cols(A). x=A''*y -> x'=y'*A'
		Mat tmp_data, tmp_mean = repeat(mean, data.rows / mean.rows, data.cols / mean.cols);
		if ( data.type() != ctype || tmp_mean.data == mean.data ) {
			data.convertTo( tmp_data, ctype );
			subtract( tmp_data, tmp_mean, tmp_data );
		} else {
			subtract( data, tmp_mean, tmp_mean );
			tmp_data = tmp_mean;
		}

		Mat evects1(count, len, ctype);
		gemm( eigenvectors, tmp_data, 1, Mat(), 0, evects1,
			  (flags & CV_PCA_DATA_AS_COL) ? CV_GEMM_B_T : 0);
		eigenvectors = evects1;

		// normalize eigenvectors
		for ( i = 0; i < out_count; i++ ) {
			Mat vec = eigenvectors.row(i);
			normalize(vec, vec);
		}
	}

	if ( count > out_count ) {
		// use clone() to physically copy the data and thus deallocate the original matrices
		eigenvalues = eigenvalues.rowRange(0, out_count).clone();
		eigenvectors = eigenvectors.rowRange(0, out_count).clone();
	}
	return *this;
}


void PCA::project(const Mat& data, Mat& result) const {
	CV_Assert( mean.data && eigenvectors.data &&
			   ((mean.rows == 1 && mean.cols == data.cols) || (mean.cols == 1 && mean.rows == data.rows)));
	Mat tmp_data, tmp_mean = repeat(mean, data.rows / mean.rows, data.cols / mean.cols);
	int ctype = mean.type();
	if ( data.type() != ctype || tmp_mean.data == mean.data ) {
		data.convertTo( tmp_data, ctype );
		subtract( tmp_data, tmp_mean, tmp_data );
	} else {
		subtract( data, tmp_mean, tmp_mean );
		tmp_data = tmp_mean;
	}
	if ( mean.rows == 1 ) {
		gemm( tmp_data, eigenvectors, 1, Mat(), 0, result, GEMM_2_T );
	} else {
		gemm( eigenvectors, tmp_data, 1, Mat(), 0, result, 0 );
	}
}

Mat PCA::project(const Mat& data) const {
	Mat result;
	project(data, result);
	return result;
}

void PCA::backProject(const Mat& data, Mat& result) const {
	CV_Assert( mean.data && eigenvectors.data &&
			   ((mean.rows == 1 && eigenvectors.rows == data.cols) ||
				(mean.cols == 1 && eigenvectors.rows == data.rows)));

	Mat tmp_data, tmp_mean;
	data.convertTo(tmp_data, mean.type());
	if ( mean.rows == 1 ) {
		tmp_mean = repeat(mean, data.rows, 1);
		gemm( tmp_data, eigenvectors, 1, tmp_mean, 1, result, 0 );
	} else {
		tmp_mean = repeat(mean, 1, data.cols);
		gemm( eigenvectors, tmp_data, 1, tmp_mean, 1, result, GEMM_1_T );
	}
}

Mat PCA::backProject(const Mat& data) const {
	Mat result;
	backProject(data, result);
	return result;
}

}

/****************************************************************************************\
*                                    Earlier API                                         *
\****************************************************************************************/

CV_IMPL void cvGEMM( const CvArr* Aarr, const CvArr* Barr, double alpha,
					 const CvArr* Carr, double beta, CvArr* Darr, int flags ) {
	cv::Mat A = cv::cvarrToMat(Aarr), B = cv::cvarrToMat(Barr);
	cv::Mat C, D = cv::cvarrToMat(Darr);

	if ( Carr ) {
		C = cv::cvarrToMat(Carr);
	}

	CV_Assert( (D.rows == ((flags & CV_GEMM_A_T) == 0 ? A.rows : A.cols)) &&
			   (D.cols == ((flags & CV_GEMM_B_T) == 0 ? B.cols : B.rows)) &&
			   D.type() == A.type() );

	gemm( A, B, alpha, C, beta, D, flags );
}


CV_IMPL void
cvTransform( const CvArr* srcarr, CvArr* dstarr,
			 const CvMat* transmat, const CvMat* shiftvec ) {
	cv::Mat m = cv::cvarrToMat(transmat), src = cv::cvarrToMat(srcarr), dst = cv::cvarrToMat(dstarr);

	if ( shiftvec ) {
		cv::Mat v = cv::cvarrToMat(shiftvec).reshape(1, m.rows),
				_m(m.rows, m.cols + 1, m.type()), m1 = _m.colRange(0, m.cols), v1 = _m.col(m.cols);
		m.convertTo(m1, m1.type());
		v.convertTo(v1, v1.type());
		m = _m;
	}

	CV_Assert( dst.depth() == src.depth() && dst.channels() == m.rows );
	cv::transform( src, dst, m );
}


CV_IMPL void
cvPerspectiveTransform( const CvArr* srcarr, CvArr* dstarr, const CvMat* mat ) {
	cv::Mat m = cv::cvarrToMat(mat), src = cv::cvarrToMat(srcarr), dst = cv::cvarrToMat(dstarr);

	CV_Assert( dst.type() == src.type() && dst.channels() == m.rows - 1 );
	cv::perspectiveTransform( src, dst, m );
}


CV_IMPL void cvScaleAdd( const CvArr* srcarr1, CvScalar scale,
						 const CvArr* srcarr2, CvArr* dstarr ) {
	cv::Mat src1 = cv::cvarrToMat(srcarr1), dst = cv::cvarrToMat(dstarr);

	CV_Assert( src1.size() == dst.size() && src1.type() == dst.type() );
	cv::scaleAdd( src1, scale.val[0], cv::cvarrToMat(srcarr2), dst );
}


CV_IMPL void
cvCalcCovarMatrix( const CvArr** vecarr, int count,
				   CvArr* covarr, CvArr* avgarr, int flags ) {
	cv::Mat cov0 = cv::cvarrToMat(covarr), cov = cov0, mean0, mean;
	CV_Assert( vecarr != 0 && count >= 1 );

	if ( avgarr ) {
		mean = mean0 = cv::cvarrToMat(avgarr);
	}

	if ( (flags & CV_COVAR_COLS) != 0 || (flags & CV_COVAR_ROWS) != 0 ) {

		cv::Mat data = cv::cvarrToMat(vecarr[0]);
		cv::calcCovarMatrix( data, cov, mean, flags, cov.type() );
	} else {
		std::vector<cv::Mat> data(count);
		for ( int i = 0; i < count; i++ ) {
			data[i] = cv::cvarrToMat(vecarr[i]);
		}
		cv::calcCovarMatrix( &data[0], count, cov, mean, flags, cov.type() );
	}

	if ( mean.data != mean0.data && mean0.data ) {
		mean.convertTo(mean0, mean0.type());
	}

	if ( cov.data != cov0.data ) {
		cov.convertTo(cov0, cov0.type());
	}
}


CV_IMPL double
cvMahalanobis( const CvArr* srcAarr, const CvArr* srcBarr, const CvArr* matarr ) {
	return cv::Mahalanobis(cv::cvarrToMat(srcAarr),
						   cv::cvarrToMat(srcBarr), cv::cvarrToMat(matarr));
}

CV_IMPL void
cvMulTransposed( const CvArr* srcarr, CvArr* dstarr,
				 int order, const CvArr* deltaarr, double scale ) {
	cv::Mat src = cv::cvarrToMat(srcarr), dst0 = cv::cvarrToMat(dstarr), dst = dst0, delta;
	if ( deltaarr ) {
		delta = cv::cvarrToMat(deltaarr);
	}
	cv::mulTransposed( src, dst, order != 0, delta, scale, dst.type());
	if ( dst.data != dst0.data ) {
		dst.convertTo(dst0, dst0.type());
	}
}

CV_IMPL double cvDotProduct( const CvArr* srcAarr, const CvArr* srcBarr ) {
	return cv::cvarrToMat(srcAarr).dot(cv::cvarrToMat(srcBarr));
}


CV_IMPL void
cvCalcPCA( const CvArr* data_arr, CvArr* avg_arr, CvArr* eigenvals, CvArr* eigenvects, int flags ) {
	cv::Mat data = cv::cvarrToMat(data_arr), mean0 = cv::cvarrToMat(avg_arr);
	cv::Mat evals0 = cv::cvarrToMat(eigenvals), evects0 = cv::cvarrToMat(eigenvects);
	cv::Mat mean = mean0, evals = evals0, evects = evects0;

	cv::PCA pca;
	pca.mean = mean;
	pca.eigenvalues = evals;
	pca.eigenvectors = evects;

	pca(data, (flags & CV_PCA_USE_AVG) ? mean : cv::Mat(),
		flags, evals.data ? evals.rows + evals.cols - 1 : 0);

	if ( pca.mean.size() == mean.size() ) {
		pca.mean.convertTo( mean, mean.type() );
	} else {
		cv::Mat temp; pca.mean.convertTo( temp, mean.type() );
		transpose( temp, mean );
	}

	evals = pca.eigenvalues;
	evects = pca.eigenvectors;
	int ecount0 = evals0.cols + evals0.rows - 1;
	int ecount = evals.cols + evals.rows - 1;

	CV_Assert( (evals0.cols == 1 || evals0.rows == 1) &&
			   ecount0 <= ecount &&
			   evects0.cols == evects.cols &&
			   evects0.rows == ecount0 );

	cv::Mat temp = evals0;
	if ( evals.rows == 1 ) {
		evals.colRange(0, ecount0).convertTo(temp, evals0.type());
	} else {
		evals.rowRange(0, ecount0).convertTo(temp, evals0.type());
	}
	if ( temp.data != evals0.data ) {
		transpose(temp, evals0);
	}
	evects.rowRange(0, ecount0).convertTo( evects0, evects0.type() );

	// otherwise some datatype's or size's were incorrect, so the output arrays have been reallocated
	CV_Assert( mean0.data == mean.data );
}


CV_IMPL void
cvProjectPCA( const CvArr* data_arr, const CvArr* avg_arr,
			  const CvArr* eigenvects, CvArr* result_arr ) {
	cv::Mat data = cv::cvarrToMat(data_arr), mean = cv::cvarrToMat(avg_arr);
	cv::Mat evects = cv::cvarrToMat(eigenvects), dst0 = cv::cvarrToMat(result_arr), dst = dst0;

	cv::PCA pca;
	pca.mean = mean;
	int n;
	if ( mean.rows == 1 ) {
		CV_Assert(dst.cols <= evects.rows && dst.rows == data.rows);
		n = dst.cols;
	} else {
		CV_Assert(dst.rows <= evects.rows && dst.cols == data.cols);
		n = dst.rows;
	}
	pca.eigenvectors = evects.rowRange(0, n);

	cv::Mat result = pca.project(data);
	if ( result.cols != dst.cols ) {
		result = result.reshape(1, 1);
	}
	result.convertTo(dst, dst.type());

	CV_Assert(dst0.data == dst.data);
}


CV_IMPL void
cvBackProjectPCA( const CvArr* proj_arr, const CvArr* avg_arr,
				  const CvArr* eigenvects, CvArr* result_arr ) {
	cv::Mat data = cv::cvarrToMat(proj_arr), mean = cv::cvarrToMat(avg_arr);
	cv::Mat evects = cv::cvarrToMat(eigenvects), dst0 = cv::cvarrToMat(result_arr), dst = dst0;

	cv::PCA pca;
	pca.mean = mean;
	int n;
	if ( mean.rows == 1 ) {
		CV_Assert(data.cols <= evects.rows && dst.rows == data.rows);
		n = data.cols;
	} else {
		CV_Assert(data.rows <= evects.rows && dst.cols == data.cols);
		n = data.rows;
	}
	pca.eigenvectors = evects.rowRange(0, n);

	cv::Mat result = pca.backProject(data);
	result.convertTo(dst, dst.type());

	CV_Assert(dst0.data == dst.data);
}

/* End of file. */
